trick/trick_sims/SIM_robot/models/kinematics/ManipKinemat.cc
Sean Harmeyer fd8252e2c5
Sim robot (#1498)
* 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>
2023-05-12 10:46:51 -05:00

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;
}