void fixQuaternionSign(const Eigen::Quaterniond& q1, Eigen::Quaterniond& q2,
const Eigen::Vector3d& w1, Eigen::Vector3d& w2,
const Eigen::Vector3d& a1, Eigen::Vector3d& a2)
{
SL_quat sl_q1, sl_q2;
floating_base_utilities::GeometryUtils::EigQuaternionToSL(q1, sl_q1.q);
floating_base_utilities::GeometryUtils::EigQuaternionToSL(q2, sl_q2.q);
Eigen::Map<Eigen::Vector3d>(&(sl_q1.ad[1])) = w1;
Eigen::Map<Eigen::Vector3d>(&(sl_q2.ad[1])) = w2;
Eigen::Map<Eigen::Vector3d>(&(sl_q1.add[1])) = a1;
Eigen::Map<Eigen::Vector3d>(&(sl_q2.add[1])) = a2;
quatDerivatives(&sl_q1);
quatDerivatives(&sl_q2);
fixQuaternionSign(sl_q1, sl_q2);
//map back the values
floating_base_utilities::GeometryUtils::SLQuaternionToEigen(sl_q2.q, q2);
w2 = Eigen::Map<Eigen::Vector3d>(&(sl_q2.ad[1]));
a2 = Eigen::Map<Eigen::Vector3d>(&(sl_q2.add[1]));
}
/*!*****************************************************************************
*******************************************************************************
\note compute_kinematics
\date June 99
\remarks
computes kinematic variables
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static void
compute_kinematics(void)
{
int i,j,r,o;
static int firsttime = TRUE;
static SL_DJstate *temp;
static Matrix last_J;
static Matrix last_Jbase;
if (firsttime) {
temp=(SL_DJstate *)my_calloc((unsigned long)(n_dofs+1),sizeof(SL_DJstate),MY_STOP);
last_J = my_matrix(1,6*n_endeffs,1,n_dofs);
last_Jbase = my_matrix(1,6*n_endeffs,1,2*N_CART);
}
/* compute the desired link positions */
linkInformationDes(joint_des_state,&base_state,&base_orient,endeff,
joint_cog_mpos_des,joint_axis_pos_des,joint_origin_pos_des,
link_pos_des,Alink_des,Adof_des);
/* the desired endeffector information */
for (i=1; i<=N_CART; ++i) {
for (j=1; j<=n_endeffs; ++j) {
cart_des_state[j].x[i] = link_pos_des[link2endeffmap[j]][i];
}
}
/* the desired quaternian of the endeffector */
for (j=1; j<=n_endeffs; ++j) {
linkQuat(Alink_des[link2endeffmap[j]],&(cart_des_orient[j]));
}
/* addititional link information */
linkInformation(joint_state,&base_state,&base_orient,endeff,
joint_cog_mpos,joint_axis_pos,joint_origin_pos,
link_pos,Alink,Adof);
/* create the endeffector information */
for (i=1; i<=N_CART; ++i) {
for (j=1; j<=n_endeffs; ++j) {
cart_state[j].x[i] = link_pos[link2endeffmap[j]][i];
}
}
/* the quaternian of the endeffector */
for (j=1; j<=n_endeffs; ++j) {
linkQuat(Alink[link2endeffmap[j]],&(cart_orient[j]));
}
/* the COG position */
compute_cog();
/* the jacobian */
jacobian(link_pos,joint_origin_pos,joint_axis_pos,J);
baseJacobian(link_pos,joint_origin_pos,joint_axis_pos,Jbase);
jacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jdes);
baseJacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jbasedes);
/* numerical time derivative of Jacobian */
if (!firsttime) {
mat_sub(J,last_J,dJdt);
mat_mult_scalar(dJdt,(double)ros_servo_rate,dJdt);
mat_sub(Jbase,last_Jbase,dJbasedt);
mat_mult_scalar(dJbasedt,(double)ros_servo_rate,dJbasedt);
}
mat_equal(J,last_J);
mat_equal(Jbase,last_Jbase);
/* compute the cartesian velocities and accelerations */
for (j=1; j<=n_endeffs; ++j) {
for (i=1; i<=N_CART; ++i) {
cart_state[j].xd[i] = 0.0;
cart_state[j].xdd[i] = 0.0;
cart_orient[j].ad[i] = 0.0;
cart_orient[j].add[i] = 0.0;
cart_des_state[j].xd[i] = 0.0;
cart_des_orient[j].ad[i] = 0.0;
/* contributations from the joints */
for (r=1; r<=n_dofs; ++r) {
cart_state[j].xd[i] += J[(j-1)*6+i][r] * joint_state[r].thd;
cart_orient[j].ad[i] += J[(j-1)*6+i+3][r] * joint_state[r].thd;
cart_des_state[j].xd[i] += Jdes[(j-1)*6+i][r] *joint_des_state[r].thd;
cart_des_orient[j].ad[i]+= Jdes[(j-1)*6+i+3][r] * joint_des_state[r].thd;
cart_state[j].xdd[i] += J[(j-1)*6+i][r] * joint_state[r].thdd +
dJdt[(j-1)*6+i][r] * joint_state[r].thd;
cart_orient[j].add[i] += J[(j-1)*6+i+3][r] * joint_state[r].thdd +
dJdt[(j-1)*6+i+3][r] * joint_state[r].thd;
}
/* contributations from the base */
for (r=1; r<=N_CART; ++r) {
cart_state[j].xd[i] += Jbase[(j-1)*6+i][r] * base_state.xd[r];
cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][r] * base_state.xd[r];
cart_state[j].xd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.ad[r];
cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][r] * base_state.xd[r];
cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][r] * base_state.xd[r];
cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][3+r] * base_orient.ad[r];
cart_state[j].xdd[i] += Jbase[(j-1)*6+i][r] * base_state.xdd[r] +
dJbasedt[(j-1)*6+i][r] * base_state.xd[r];
cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][r] * base_state.xdd[r] +
dJbasedt[(j-1)*6+i+3][r] * base_state.xd[r];
cart_state[j].xdd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.add[r] +
dJbasedt[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.add[r] +
dJbasedt[(j-1)*6+i+3][3+r] * base_orient.ad[r];
}
}
/* compute quaternion derivatives */
quatDerivatives(&(cart_orient[j]));
quatDerivatives(&(cart_des_orient[j]));
for (r=1; r<=N_QUAT; ++r)
cart_des_orient[j].qdd[r] = 0.0; // we don't have dJdes_dt so far
}
/* reset first time flag */
firsttime = FALSE;
}
/*!*****************************************************************************
*******************************************************************************
\note SL_IntegrateEuler
\date June 1999
\remarks
Euler integration
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in,out] state : the state and commands used for integration
\param[in,out] cbase : the position state of the base
\param[in,out] obase : the orientational state of the base
\param[in] ux : the external forces acting on each joint
\param[in] leff : the leffector parameters
\param[in] dt : the time step
\param[in] ndofs : the number of DOFS
\param[in] flag : compute forward dynamics yes/no
******************************************************************************/
void
SL_IntegrateEuler(SL_Jstate *state, SL_Cstate *cbase,
SL_quat *obase, SL_uext *ux,
SL_endeff *leff, double dt, int ndofs,
int flag)
{
register int i;
double aux=0;
static int firsttime = TRUE;
if (firsttime) {
addToMan("freezeBase","freeze the base at orgin",freezeBaseToggle);
firsttime = FALSE;
}
if (flag) {
// compute the accelerations
SL_ForDyn(state, cbase, obase, ux, leff);
}
// Euler integrate forward
// the DOFs
for(i=1; i<=ndofs; i++) {
state[i].thd += dt*state[i].thdd;
state[i].th += dt*state[i].thd;
}
if (!freeze_base) { // optional freezing of base coordinates
// translations of the base
for(i=1; i<=N_CART; i++) {
cbase->xd[i] += dt*cbase->xdd[i];
cbase->x[i] += dt*cbase->xd[i];
}
// orientation of the base in quaternions
for(i=1; i<=N_CART; i++) {
obase->ad[i] += dt*obase->add[i];
}
// compute quaternion velocity and acceleration
quatDerivatives(obase);
// integrate to obtain new angular orientation
for(i=1; i<=N_QUAT; i++) {
obase->q[i] += dt*obase->qd[i];
aux += sqr(obase->q[i]);
}
aux = sqrt(aux);
if (aux == 0)
aux = 1.e-10;
// important: renormalize quaternions
for(i=1; i<=N_QUAT; i++) {
obase->q[i]/=aux;
}
} else { // if base coordinates are frozen
bzero((void *)cbase,sizeof(SL_Cstate));
bzero((void *)obase,sizeof(SL_quat));
obase->q[_Q0_] = freeze_base_quat[_Q0_];
obase->q[_Q1_] = freeze_base_quat[_Q1_];
obase->q[_Q2_] = freeze_base_quat[_Q2_];
obase->q[_Q3_] = freeze_base_quat[_Q3_];
cbase->x[_X_] = freeze_base_pos[_X_];
cbase->x[_Y_] = freeze_base_pos[_Y_];
cbase->x[_Z_] = freeze_base_pos[_Z_];
}
// update the simulated link positions such that contact forces are correct
linkInformation(state,cbase,obase,leff,
joint_cog_mpos_sim,joint_axis_pos_sim,joint_origin_pos_sim,
link_pos_sim,Alink_sim,Adof_sim);
// check for contacts with objects
checkContacts();
}
