mirror of
https://github.com/nasa/trick.git
synced 2024-12-24 23:36:43 +00:00
fd8252e2c5
* First commit of SIM_robot. * Updates to kinematic arm with controller * Working well enough, still needs some user interface besides trick-TV * Add end-effector path trace to graphics client. * Singularity control bug, remove printouts, udpate makefile * Improve SIM_robot variable server client. * Tidy up RobotDisplay.java * Tidying up * Removing warnings * Working on documentation * Updating documentation * Updating docs * Adding figures for documentation * Removing some stuff in the README carried over from the template * Tidying up * Position vector finally done * Updating based on feedback * Forward position kinematics completed with notation changes * First pass documentation done? * remove printout * Fix typos in text and filenames * Update README.md * Update README.md * made it smaller --------- Co-authored-by: John M. Penn <john.m.penn@nasa.gov>
265 lines
7.5 KiB
C++
265 lines
7.5 KiB
C++
/********************************* TRICK HEADER *******************************
|
|
PURPOSE: ( Simulate a planar, 2DOF manipulator for Trick example sim.)
|
|
LIBRARY DEPENDENCY:
|
|
((kinematics/ManipKinemat.o)
|
|
(utils/utils.o)
|
|
)
|
|
PROGRAMMERS:
|
|
(((Sean Harmeyer) (NASA) (Dec 2022) (Trick Example Sim)))
|
|
*******************************************************************************/
|
|
|
|
#include "kinematics/ManipKinemat.hh"
|
|
#include "include/trick/memorymanager_c_intf.h"
|
|
|
|
ManipKinemat::ManipKinemat(int numDof):ndof(numDof)
|
|
{
|
|
|
|
int i,j;
|
|
|
|
/* These vectors and matrices don't explicitly depend on the
|
|
* number of degrees of freedom */
|
|
V_ee = (double *)TMM_declare_var_1d("double",2);
|
|
P_task_base = (double *)TMM_declare_var_1d("double",2);
|
|
P_joint_ee = (double *)TMM_declare_var_1d("double",2);
|
|
P_task_ee = (double *)TMM_declare_var_1d("double",2);
|
|
joint_ee = (double *)TMM_declare_var_1d("double",2);
|
|
|
|
R_task_base = (double **)TMM_declare_var_1d("double*",2);
|
|
R_base_task = (double **)TMM_declare_var_1d("double*",2);
|
|
R_ee_joint = (double **)TMM_declare_var_1d("double*",2);
|
|
jacobian = (double **)TMM_declare_var_1d("double*",2);
|
|
R_ee_task = (double **)TMM_declare_var_1d("double*",2);
|
|
|
|
/* total number of degrees of freedom determines the size of all of these
|
|
* vectors and matrices, so we size them accordingly */
|
|
|
|
joint_u = (double *)TMM_declare_var_1d("double",ndof);
|
|
joint_q = (double *)TMM_declare_var_1d("double",ndof);
|
|
joint_w = (double *)TMM_declare_var_1d("double",ndof);
|
|
|
|
joint_l = (double **)TMM_declare_var_1d("double*",ndof);
|
|
jacInv = (double **)TMM_declare_var_1d("double*",ndof);
|
|
|
|
P_joint_joint = (double **)TMM_declare_var_1d("double*",ndof);
|
|
P_task_joint = (double **)TMM_declare_var_1d("double*",ndof);
|
|
V_joint_joint = (double **)TMM_declare_var_1d("double*",ndof);
|
|
V_joint_task = (double **)TMM_declare_var_1d("double*",ndof);
|
|
|
|
R_joint = (double ***)TMM_declare_var_1d("double**",ndof);
|
|
R_joint_task = (double ***)TMM_declare_var_1d("double**",ndof);
|
|
|
|
for(i=0;i<2;i++)
|
|
{
|
|
R_task_base[i] = (double *)TMM_declare_var_1d("double",2);
|
|
R_base_task[i] = (double *)TMM_declare_var_1d("double",2);
|
|
R_ee_task[i] = (double *)TMM_declare_var_1d("double",2);
|
|
R_ee_joint[i] = (double *)TMM_declare_var_1d("double",2);
|
|
jacobian[i] = (double *)TMM_declare_var_1d("double",ndof);
|
|
}
|
|
|
|
|
|
for(i=0;i<ndof;i++)
|
|
{
|
|
|
|
joint_l[i] = (double *)TMM_declare_var_1d("double",2);
|
|
P_joint_joint[i] = (double *)TMM_declare_var_1d("double",2);
|
|
P_task_joint[i] = (double *)TMM_declare_var_1d("double",2);
|
|
V_joint_joint[i] = (double *)TMM_declare_var_1d("double",2);
|
|
V_joint_task[i] = (double *)TMM_declare_var_1d("double",2);
|
|
|
|
R_joint[i] = (double **)TMM_declare_var_1d("double*",2);
|
|
R_joint_task[i] = (double **)TMM_declare_var_1d("double*",2);
|
|
jacInv[i] = (double *)TMM_declare_var_1d("double",2);
|
|
|
|
for(j=0;j<ndof;j++)
|
|
{
|
|
R_joint[i][j] = (double *)TMM_declare_var_1d("double",2);
|
|
R_joint_task[i][j] = (double *)TMM_declare_var_1d("double",2);
|
|
}
|
|
|
|
}
|
|
|
|
/* base relative to task in task frame*/
|
|
P_task_base[0] = 0;
|
|
P_task_base[1] = 0;
|
|
|
|
R_base_task[0][0] = 1;
|
|
R_base_task[0][1] = 0;
|
|
R_base_task[1][0] = 0;
|
|
R_base_task[1][1] = 1;
|
|
|
|
R_task_base[0][0] = 1;
|
|
R_task_base[0][1] = 0;
|
|
R_task_base[1][0] = 0;
|
|
R_task_base[1][1] = 1;
|
|
|
|
|
|
/* initial joint angles */
|
|
joint_q[0] = 0;
|
|
joint_q[1] = 0;
|
|
|
|
/* link lengths */
|
|
joint_l[0][0] = 2;
|
|
joint_l[0][1] = 0;
|
|
joint_l[1][0] = 2;
|
|
joint_l[1][1] = 0;
|
|
|
|
/* EE offset */
|
|
joint_ee[0] = 0;
|
|
joint_ee[1] = 0;
|
|
|
|
/* Rotation from EE to previous joint */
|
|
R_ee_joint[0][0] = 1;
|
|
R_ee_joint[0][1] = 0;
|
|
R_ee_joint[1][0] = 0;
|
|
R_ee_joint[1][1] = 1;
|
|
|
|
jacobian[0][0] = 0;
|
|
jacobian[0][1] = 0;
|
|
jacobian[1][0] = 0;
|
|
jacobian[1][1] = 0;
|
|
|
|
jacDet = 0;
|
|
|
|
singularityTolerance = .17453;
|
|
checkSingularities = true;
|
|
|
|
}
|
|
|
|
void ManipKinemat::forwardKinPos()
|
|
{
|
|
int i; // loop counter
|
|
double c,s; // sine and cosine shorthand
|
|
|
|
// rescale angles to 0 < q < 2*PI
|
|
for(i=0; i<ndof; i++)
|
|
{
|
|
if (joint_q[i] > 2.0 * M_PI)
|
|
joint_q[i] -= 2.0 * M_PI;
|
|
else if (joint_q[i] < -2.0 * M_PI)
|
|
joint_q[i] += 2.0 * M_PI;
|
|
}
|
|
|
|
// Calculate current rotation matrices for all frames
|
|
for (i=0; i<ndof; i++)
|
|
{
|
|
c = cos(joint_q[i]);
|
|
s = sin(joint_q[i]);
|
|
|
|
R_joint[i][0][0]= c;
|
|
R_joint[i][0][1]= -s;
|
|
R_joint[i][1][0]= s;
|
|
R_joint[i][1][1]= c;
|
|
|
|
// Calculate position of every joint tip in previous frame
|
|
utils.MVMult(P_joint_joint[i], (double** )R_joint[i], joint_l[i]);
|
|
}
|
|
|
|
|
|
// Calculate R from every joint back to task
|
|
utils.Transpose(R_base_task, R_task_base);
|
|
|
|
for(i=0;i<ndof;i++)
|
|
{
|
|
if(i == 0)
|
|
utils.MMMult(R_joint_task[i], R_base_task, R_joint[i]);
|
|
else
|
|
utils.MMMult(R_joint_task[i], R_joint_task[i-1], R_joint[i]);
|
|
}
|
|
|
|
// Calculate R from EE to task
|
|
utils.MMMult(R_ee_task, R_joint_task[ndof-1], R_ee_joint);
|
|
|
|
|
|
|
|
// Calculate vector from task to each joint in task
|
|
for(i=0;i<ndof;i++)
|
|
{
|
|
if (i==0)
|
|
utils.MVMult(P_task_joint[i],R_joint_task[i],joint_l[i]);
|
|
else
|
|
{
|
|
utils.MVMult(P_task_joint[i],R_joint_task[i],joint_l[i]);
|
|
utils.VAdd(P_task_joint[i],P_task_joint[i],P_task_joint[i-1]);
|
|
}
|
|
}
|
|
|
|
// Calculate vector from task to EE in task
|
|
|
|
utils.MVMult(P_task_ee,R_ee_task,joint_ee);
|
|
utils.VAdd(P_task_ee,P_task_ee,P_task_joint[ndof-1]);
|
|
|
|
|
|
}
|
|
|
|
|
|
void ManipKinemat::forwardKinVel()
|
|
{
|
|
|
|
/* like the jacobian, these are hand-calculated for a 2-dof planar.
|
|
* Anything more would require velocity propagation to determine the
|
|
* velocity of the end-effector and intermediate points */
|
|
|
|
double l1, l2, s1, c1, s12, c12;
|
|
l1 = joint_l[0][0];
|
|
l2 = joint_l[1][0];
|
|
|
|
s1 = sin(joint_q[0]);
|
|
s12 = sin(joint_q[0]+joint_q[1]);
|
|
|
|
c1 = cos(joint_q[0]);
|
|
c12 = cos(joint_q[0]+joint_q[1]);
|
|
|
|
|
|
V_ee[0] = ( -l1*s1 - l2*s12 )*joint_w[0] - l2*s12*joint_w[1];
|
|
V_ee[1] = ( l1*c1 + l2*c12 )*joint_w[0] + l2*c12*joint_w[1];
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
void ManipKinemat::calcJacobian()
|
|
{
|
|
|
|
/* These are hand-calculated due to the simplicity of the manipulator
|
|
* system. The algorithmic method of determining the elements of
|
|
* the jacobian, while efficient and generic, are difficult to read in
|
|
* code and are beyond the scope of a simple robotic tutorial
|
|
*/
|
|
|
|
double s1,c1,s12,c12;
|
|
s1 = sin(joint_q[0]);
|
|
s12 = sin(joint_q[0]+joint_q[1]);
|
|
c1 = cos(joint_q[0]);
|
|
c12 = cos(joint_q[0]+joint_q[1]);
|
|
|
|
double l1, l2;
|
|
l1 = joint_l[0][0];
|
|
l2 = joint_l[1][0];
|
|
|
|
jacobian[0][0] = -l1*s1 - l2*s12;
|
|
jacobian[0][1] = -l2*s12;
|
|
jacobian[1][0] = l1*c1 + l2*c12;
|
|
jacobian[1][1] = l2*c12;
|
|
|
|
|
|
/* Calculate the inverse jacobian as well */
|
|
|
|
jacDet = jacobian[0][0]*jacobian[1][1] - jacobian[0][1]*jacobian[1][0];
|
|
|
|
jacInv[0][0] = jacobian[1][1]/jacDet;
|
|
jacInv[0][1] = -jacobian[0][1]/jacDet;
|
|
jacInv[1][0] = -jacobian[1][0]/jacDet;
|
|
jacInv[1][1] = jacobian[0][0]/jacDet;
|
|
|
|
}
|
|
|
|
bool ManipKinemat::isSingular()
|
|
{
|
|
if( (fabs(joint_q[1]) < singularityTolerance) || ( fabs( fabs(joint_q[1]) - M_PI) < singularityTolerance ) )
|
|
return true;
|
|
else
|
|
return false;
|
|
}
|