void FreeJoint::_updateVelocity()
{
// S
Eigen::Vector3d q(mCoordinate[0].get_q(),
mCoordinate[1].get_q(),
mCoordinate[2].get_q());
Eigen::Matrix3d J = math::expMapJac(q);
Eigen::Vector6d J0;
Eigen::Vector6d J1;
Eigen::Vector6d J2;
Eigen::Vector6d J3;
Eigen::Vector6d J4;
Eigen::Vector6d J5;
J0 << J(0,0), J(0,1), J(0,2), 0, 0, 0;
J1 << J(1,0), J(1,1), J(1,2), 0, 0, 0;
J2 << J(2,0), J(2,1), J(2,2), 0, 0, 0;
J3 << 0, 0, 0, 1, 0, 0;
J4 << 0, 0, 0, 0, 1, 0;
J5 << 0, 0, 0, 0, 0, 1;
mS.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
mS.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
mS.col(2) = math::AdT(mT_ChildBodyToJoint, J2);
mS.col(3) = math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J3);
mS.col(4) = math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J4);
mS.col(5) = math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J5);
// V = S * dq
mV = mS * get_dq();
}
inline void UniversalJoint::_updateAcceleration()
{
// dS
mdS.col(0) = -math::ad(mS.col(1)*mCoordinate[1].get_dq(), math::AdTAngular(mT_ChildBodyToJoint * math::expAngular(-mAxis[1]*mCoordinate[1].get_q()), mAxis[0]));
//mdS.col(1) = setZero();
// dV = dS * dq + S * ddq
mdV = mdS * get_dq() + mS * get_ddq();
}
void FreeJoint::updateTransform_Issue122(double _timeStep) {
mT_Joint = mT_Joint * math::expMap(_timeStep * get_dq());
set_q(math::logMap(mT_Joint));
mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse();
assert(math::verifyTransform(mT));
}
inline void UniversalJoint::_updateVelocity()
{
// S
mS.col(0) = math::AdTAngular(mT_ChildBodyToJoint*math::expAngular(-mAxis[1]*mCoordinate[1].get_q()), mAxis[0]);
mS.col(1) = math::AdTAngular(mT_ChildBodyToJoint, mAxis[1]);
// V = S * dq
mV = mS * get_dq();
}
void RevoluteJoint::_updateVelocity()
{
// S
mS = math::AdTAngular(mT_ChildBodyToJoint, mAxis);
// V = S * dq
mV = mS * get_dq();
//mV.setAngular(mAxis * mCoordinate.get_q());
}
void FreeJoint::updateJacobianTimeDeriv() {
Eigen::Vector3d q(mCoordinate[0].get_q(),
mCoordinate[1].get_q(),
mCoordinate[2].get_q());
Eigen::Vector3d dq(mCoordinate[0].get_dq(),
mCoordinate[1].get_dq(),
mCoordinate[2].get_dq());
Eigen::Matrix3d dJ = math::expMapJacDot(q, dq);
Eigen::Vector6d dJ0;
Eigen::Vector6d dJ1;
Eigen::Vector6d dJ2;
Eigen::Vector6d J3;
Eigen::Vector6d J4;
Eigen::Vector6d J5;
dJ0 << dJ(0, 0), dJ(0, 1), dJ(0, 2), 0, 0, 0;
dJ1 << dJ(1, 0), dJ(1, 1), dJ(1, 2), 0, 0, 0;
dJ2 << dJ(2, 0), dJ(2, 1), dJ(2, 2), 0, 0, 0;
J3 << 0, 0, 0, 1, 0, 0;
J4 << 0, 0, 0, 0, 1, 0;
J5 << 0, 0, 0, 0, 0, 1;
mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0);
mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1);
mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2);
mdS.col(3) =
-math::ad(mS.leftCols<3>() * get_dq().head<3>(),
math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J3));
mdS.col(4) =
-math::ad(mS.leftCols<3>() * get_dq().head<3>(),
math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J4));
mdS.col(5) =
-math::ad(mS.leftCols<3>() * get_dq().head<3>(),
math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J5));
assert(!math::isNan(mdS));
}
void FreeJoint::_updateAcceleration()
{
// dS
Eigen::Vector3d q(mCoordinate[0].get_q(),
mCoordinate[1].get_q(),
mCoordinate[2].get_q());
Eigen::Vector3d dq(mCoordinate[0].get_dq(),
mCoordinate[1].get_dq(),
mCoordinate[2].get_dq());
Eigen::Matrix3d dJ = math::expMapJacDot(q, dq);
Eigen::Vector6d dJ0;
Eigen::Vector6d dJ1;
Eigen::Vector6d dJ2;
Eigen::Vector6d J3;
Eigen::Vector6d J4;
Eigen::Vector6d J5;
dJ0 << dJ(0,0), dJ(0,1), dJ(0,2), 0, 0, 0;
dJ1 << dJ(1,0), dJ(1,1), dJ(1,2), 0, 0, 0;
dJ2 << dJ(2,0), dJ(2,1), dJ(2,2), 0, 0, 0;
J3 << 0, 0, 0, 1, 0, 0;
J4 << 0, 0, 0, 0, 1, 0;
J5 << 0, 0, 0, 0, 0, 1;
mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0);
mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1);
mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2);
mdS.col(3) = -math::ad(mS.leftCols<3>() * get_dq().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J3));
mdS.col(4) = -math::ad(mS.leftCols<3>() * get_dq().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J4));
mdS.col(5) = -math::ad(mS.leftCols<3>() * get_dq().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J5));
// dV = dS * dq + S * ddq
mdV = mdS * get_dq() + mS * get_ddq();
}
void TranslationalJoint::_updateVelocity()
{
// S
Eigen::Vector6d J0;
Eigen::Vector6d J1;
Eigen::Vector6d J2;
J0 << 0, 0, 0, 1, 0, 0;
J1 << 0, 0, 0, 0, 1, 0;
J2 << 0, 0, 0, 0, 0, 1;
mS.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
mS.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
mS.col(2) = math::AdT(mT_ChildBodyToJoint, J2);
// V = S * dq
mV = mS * get_dq();
}
inline void EulerJoint::_updateAcceleration()
{
// dS
double q0 = mCoordinate[0].get_q();
double q1 = mCoordinate[1].get_q();
double q2 = mCoordinate[2].get_q();
//double dq0 = mCoordinate[0].get_dq();
double dq1 = mCoordinate[1].get_dq();
double dq2 = mCoordinate[2].get_dq();
//double c0 = cos(q0);
double c1 = cos(q1);
double c2 = cos(q2);
//double s0 = sin(q0);
double s1 = sin(q1);
double s2 = sin(q2);
Eigen::Vector6d dJ0 = Eigen::Vector6d::Zero();
Eigen::Vector6d dJ1 = Eigen::Vector6d::Zero();
Eigen::Vector6d dJ2 = Eigen::Vector6d::Zero();
switch (mAxisOrder)
{
case AO_XYZ:
{
//--------------------------------------------------------------------------
// dS = [ -(dq1*c2*s1) - dq2*c1*s2, dq2*c2, 0
// -(dq2*c1*c2) + dq1*s1*s2, -(dq2*s2), 0
// dq1*c1, 0, 0
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//--------------------------------------------------------------------------
dJ0 << -(dq1*c2*s1) - dq2*c1*s2, -(dq2*c1*c2) + dq1*s1*s2, dq1*c1, 0, 0, 0;
dJ1 << dq2*c2, -(dq2*s2), 0.0, 0.0, 0.0, 0.0;
dJ2.setConstant(0.0);
break;
}
case AO_ZYX:
{
//--------------------------------------------------------------------------
// dS = [ -c1*dq1, 0, 0
// c2*c1*dq2-s2*s1*dq1, -s2*dq2, 0
// -s1*c2*dq1-c1*s2*dq2, -c2*dq2, 0
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//--------------------------------------------------------------------------
dJ0 << -c1*dq1, c2*c1*dq2 - s2*s1*dq1, -s1*c2*dq1 - c1*s2*dq2, 0.0, 0.0, 0.0;
dJ1 << 0.0, -s2*dq2, -c2*dq2, 0.0, 0.0, 0.0;
dJ2.setConstant(0.0);
break;
}
default:
{
dterr << "Undefined Euler axis order\n";
break;
}
}
mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0);
mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1);
mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2);
// dV = dS * dq + S * ddq
mdV = mdS * get_dq() + mS * get_ddq();
}
inline void EulerJoint::_updateVelocity()
{
// S
double q0 = mCoordinate[0].get_q();
double q1 = mCoordinate[1].get_q();
double q2 = mCoordinate[2].get_q();
//double c0 = cos(q0);
double c1 = cos(q1);
double c2 = cos(q2);
//double s0 = sin(q0);
double s1 = sin(q1);
double s2 = sin(q2);
Eigen::Vector6d J0 = Eigen::Vector6d::Zero();
Eigen::Vector6d J1 = Eigen::Vector6d::Zero();
Eigen::Vector6d J2 = Eigen::Vector6d::Zero();
switch (mAxisOrder)
{
case AO_XYZ:
{
//--------------------------------------------------------------------------
// S = [ c1*c2, s2, 0
// -(c1*s2), c2, 0
// s1, 0, 1
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//--------------------------------------------------------------------------
J0 << c1*c2, -(c1*s2), s1, 0.0, 0.0, 0.0;
J1 << s2, c2, 0.0, 0.0, 0.0, 0.0;
J2 << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
break;
}
case AO_ZYX:
{
//--------------------------------------------------------------------------
// S = [ -s1, 0, 1
// s2*c1, c2, 0
// c1*c2, -s2, 0
// 0, 0, 0
// 0, 0, 0
// 0, 0, 0 ];
//--------------------------------------------------------------------------
J0 << -s1, s2*c1, c1*c2, 0.0, 0.0, 0.0;
J1 << 0.0, c2, -s2, 0.0, 0.0, 0.0;
J2 << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
break;
}
default:
{
dterr << "Undefined Euler axis order\n";
break;
}
}
mS.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
mS.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
mS.col(2) = math::AdT(mT_ChildBodyToJoint, J2);
// V = S * dq
mV = mS * get_dq();
}
Eigen::VectorXd Skeleton::getPoseVelocity() const
{
return get_dq();
}
