bool G2oEdgeSE3
::read(std::istream& is)
{
// assert(false);
std::cout << "read G2oEdgeSE3 called" << "\n";
Vector3d vec;
for(int i=0; i<3; i++) is >> vec[i];
double q_in [4];
for(int i=0; i<4; i++) is >> q_in[i];
Quaterniond q(q_in[0], q_in[1], q_in[2], q_in[3]);
Sophus::SE3 se3(q, vec);
setMeasurement(se3);
if (is.bad()) {
return false;
}
for ( int i=0; i<information().rows() && is.good(); i++)
for (int j=i; j<information().cols() && is.good(); j++){
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
if (is.bad()) {
// we overwrite the information matrix with the Identity
information().setIdentity();
}
return true;
}
bool GSystem::setGravity(Vec3 g_)
{
if ( pGround == NULL ) return false;
pGround->dV = se3(Vec3(0,0,0),-g_);
return true;
}
void GJointRevolute::update()
{
//dS, dSdq are still zeros.
if ( bReversed ) {
T = SE3(Exp(-axis*coordinate.q), Vec3(0,0,0));
inv_T = SE3(~T.GetRotation());
Sdq = se3(-axis*coordinate.dq, Vec3(0,0,0));
Sddq = se3(-axis*coordinate.ddq, Vec3(0,0,0));
DSdqDt = Sddq;
S[0] = -axis[0]; S[1] = -axis[1]; S[2] = -axis[2];
} else {
T = SE3(Exp(axis*coordinate.q), Vec3(0,0,0));
inv_T = SE3(~T.GetRotation());
Sdq = se3(axis*coordinate.dq, Vec3(0,0,0));
Sddq = se3(axis*coordinate.ddq, Vec3(0,0,0));
DSdqDt = Sddq;
S[0] = axis[0]; S[1] = axis[1]; S[2] = axis[2];
}
}
void GJointPlanar::update()
{
//dS, dSdq are still zeros.
if ( bReversed ) {
T = SE3(1,0,0,0,1,0,0,0,1, -coordinates[0].q, -coordinates[1].q, 0);
inv_T = SE3(1,0,0,0,1,0,0,0,1, coordinates[0].q, coordinates[1].q, 0);
Sdq = se3(0, 0, 0, -coordinates[0].dq, -coordinates[1].dq, 0);
Sddq = se3(0, 0, 0, -coordinates[0].ddq, -coordinates[1].ddq, 0);
DSdqDt = Sddq;
S[3] = S[10] = -1.0;
} else {
T = SE3(1,0,0,0,1,0,0,0,1, coordinates[0].q, coordinates[1].q, 0);
inv_T = SE3(1,0,0,0,1,0,0,0,1, -coordinates[0].q, -coordinates[1].q, 0);
Sdq = se3(0, 0, 0, coordinates[0].dq, coordinates[1].dq, 0);
Sddq = se3(0, 0, 0, coordinates[0].ddq, coordinates[1].ddq, 0);
DSdqDt = Sddq;
S[3] = S[10] = 1.0;
}
}
vpRJoint::vpRJoint()
{
m_rQ = m_rDq = m_rDdq = m_rActuationTau = m_rSpringDamperTau = m_rImpulsiveTau = m_rQi = m_rK = m_rC = SCALAR_0;
m_rRestitution = SCALAR_1;
m_sS = se3(SCALAR_0, SCALAR_0, SCALAR_1, SCALAR_0, SCALAR_0, SCALAR_0);
m_bUsingDefaultAxis = true;
m_bHasUpperLimit = false;
m_bHasLowerLimit = false;
m_sO = SCALAR_0;
m_sT = SCALAR_0;
m_sVl = Axis(SCALAR_0);
}
vp1DOFJoint::vp1DOFJoint()
{
m_rQ = m_rDq = m_rDdq = m_rActuationTau = m_rSpringDamperTau = m_rImpulsiveTau = m_rQi = m_rK = m_rC = SCALAR_0;
m_rRestitution = SCALAR_1;
m_sS = se3(SCALAR_0, SCALAR_0, SCALAR_1, SCALAR_0, SCALAR_0, SCALAR_0);
m_bHasUpperLimit = false;
m_bHasLowerLimit = false;
m_sO = SCALAR_0;
m_sT = SCALAR_0;
m_sVl = SCALAR_0;
m_pTransform = NULL;
}
void GSystem::calcDerivative_MomentumCOM_Dq_Ddq(RMatrix &DHcDq_, RMatrix &DHcDdq_)
{
int i;
std::list<GBody *>::iterator iter_pbody;
std::list<GCoordinate *>::iterator iter_pcoord;
std::vector<SE3> M(pBodies.size());
std::vector<GConstraintJointLoop::JOINTLOOP_CONSTRAINT_TYPE> jlc_type(pBodies.size());
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
// save previous settings
M[i] = (*iter_pbody)->fwdJointChain.M1;
jlc_type[i] = (*iter_pbody)->fwdJointChain.jointLoopConstraintType;
// prerequisites for (*iter_pbody)->getDerivative_MomentumGlobal_Dq(...) and (*iter_pbody)->getDerivative_MomentumGlobal_Ddq(...)
(*iter_pbody)->fwdJointChain.setM(SE3());
(*iter_pbody)->fwdJointChain.jointLoopConstraintType = GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION;
(*iter_pbody)->fwdJointChain.update_J();
}
// calculate the derivatives
Vec3 p = getPositionCOMGlobal();
dse3 Hg = getMomentumGlobal();
RMatrix DHgDq(6, getNumCoordinates()), DHgDdq(6, getNumCoordinates());
dse3 DHgDqi, DHgDdqi;
for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
DHgDqi = getDerivative_MomentumGlobal_Dq(*iter_pcoord);
DHgDdqi = getDerivative_MomentumGlobal_Ddq(*iter_pcoord);
//DHgDq.Push(0, i, convert_to_RMatrix(DHgDqi));
//DHgDdq.Push(0, i, convert_to_RMatrix(DHgDdqi));
matSet(&DHgDq[6*i], DHgDqi.GetArray(), 6);
matSet(&DHgDdq[6*i], DHgDdqi.GetArray(), 6);
}
RMatrix DdAdDq_Hg(6, getNumCoordinates());
Vec3 DpDqi;
for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
DpDqi = getDerivative_PositionCOMGlobal_Dq(*iter_pcoord);
//DdAdDq_Hg.Push(0, i, convert_to_RMatrix(dAd(SE3(p), dad(se3(Vec3(0,0,0),DpDqi), Hg))));
matSet(&DdAdDq_Hg[6*i], dAd(SE3(p), dad(se3(Vec3(0,0,0),DpDqi), Hg)).GetArray(), 6);
}
DHcDq_ = dAd(SE3(p), DHgDq) + DdAdDq_Hg;
DHcDdq_ = dAd(SE3(p), DHgDdq);
// restore the previous settings
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
(*iter_pbody)->fwdJointChain.setM(M[i]);
(*iter_pbody)->fwdJointChain.setJointLoopConstraintType(jlc_type[i]);
}
}
//TODO: implement, but first remove camera parameters from vertex state
bool G2oVertexSE3
::read(std::istream& is)
{
std::cout << "read G2oVertexSE3 called" << "\n";
Vector3d vec;
for(int i=0; i<3; i++) is >> vec[i];
double q_in [4];
for(int i=0; i<4; i++) is >> q_in[i];
Quaterniond q(q_in[0], q_in[1], q_in[2], q_in[3]);
Sophus::SE3 se3(q, vec);
setEstimate(se3);
return true;
}
void vpRagdollWorld::UserInputFun(unsigned char dir)
{
static int sidx = 0;
static int jidx = 0;
static int kidx = 0;
switch ( dir )
{
case 0x26 : // up
SetGravity(Vec3(0.0, 10.0, 0.0));
break;
case 0x28: // down
SetGravity(Vec3(0.0, -10.0, 0.0));
break;
case 0x25: // left
SetGravity(Vec3(-10.0, 0.0, 0.0));
break;
case 0x27: // right
//SetGravity(Vec3(10.0, 0.0, 0.0));
break;
case 0x24: // home
SetGravity(Vec3(0.0, 0.0, 10.0));
break;
case 0x23: //end
SetGravity(Vec3(0.0, 0.0, -10.0));
break;
case 0x20: // space
sidx++;
sidx %= NUM_STONE;
stone[sidx].SetFrame(Vec3(drand(-6.0 * NUM_PUPPET, 6.0 * NUM_PUPPET), -30.0, drand(10.0, 20.0)));
stone[sidx].SetGenVelocity(se3(0, 0, 0, 0, 20, 0));
break;
case 'b':
puppet[jidx][kidx].J_rope[NUM_ROPE].Break();
kidx++;
if ( kidx == NUM_PUPPET )
{
jidx++;
if ( jidx == NUM_PUPPET ) jidx = 0;
kidx = 0;
}
break;
}
}
void vpSiloWorld::ReleaseRoof(void)
{
roof.SetFrame(Vec3(0, 0, NUM_SILO_LEVEL + 10));
roof.SetGenVelocity(se3(0));
}
void vpSiloWorld::Create(void)
{
vpMaterial::GetDefaultMaterial()->SetRestitution(0.3);
vpMaterial::GetDefaultMaterial()->SetDynamicFriction(0.3);
for ( int i = 0; i < NUM_SILO_LEVEL; i++ )
{
for ( int j = 0; j < NUM_SILO_BLOCKS; j++ )
{
block[i][j].AddGeometry(new vpBox(Vec3(1, 2, 1)));
block[i][j].SetFrame(Exp(se3(0, 0, (scalar)(j + (scalar)0.5 * (i % 2)) / (scalar)NUM_SILO_BLOCKS * M_2PI, 0, 0, 0)) * SE3(Vec3(4.4, 0, 1.01 * i)));
AddBody(&block[i][j]);
block[i][j].SetInertia(Inertia(1));
}
}
for ( int i = 0; i < NUM_BALL; i++ )
{
ball[i].AddGeometry(new vpSphere(0.25));
ball[i].SetFrame(Vec3(drand(2.0), drand(2.0), NUM_SILO_LEVEL + (25.0 / NUM_BALL) * i));
ball[i].SetGenVelocity(se3(0,0,0,drand(0.1), drand(0.1), drand(0.1)));
ball[i].SetInertia(Inertia(0.5));
AddBody(&ball[i]);
}
floor.AddGeometry(new vpBox(Vec3(100, 100, 1)));
floor.SetFrame(Vec3(0, 0, -1.0));
floor.SetGround();
AddBody(&floor);
roof.AddGeometry(new vpBox(Vec3(10, 10, 1)), Vec3(0.0, 0.0, -1));
roof.SetFrame(Vec3(0, 0, -10));
AddBody(&roof);
//SetIntegrator(VP::IMPLICIT_EULER);
SetIntegrator(VP::EULER);
SetTimeStep(0.005);
SetGravity(Vec3(0.0, 0.0, -10.0));
Initialize();
BackupState();
renderer.SetTarget(this);
if ( materialArray.size() )
{
renderer.SetMaterial(&floor, materialArray[0]);
renderer.SetMaterial(&roof, materialArray[1]);
for ( int i = 0; i < NUM_SILO_LEVEL; i++ )
{
for ( int j = 0; j < NUM_SILO_BLOCKS; j++ )
{
renderer.SetMaterial(&block[i][j], materialArray[2]);
}
}
for ( int i = 0; i < NUM_BALL; i++ )
{
renderer.SetMaterial(&ball[i], materialArray[1]);
}
}
}
void test_transformations_spherical()
{
T const input_long = 15.0;
T const input_lat = 5.0;
T const expected_long = 0.26179938779914943653855361527329;
T const expected_lat = 0.08726646259971647884618453842443;
// Can be checked using http://www.calc3d.com/ejavascriptcoordcalc.html
// (for phi use long, in radians, for theta use lat, in radians, they are listed there as "theta, phi")
T const expected_polar_x = 0.084186;
T const expected_polar_y = 0.0225576;
T const expected_polar_z = 0.996195;
// Can be checked with same URL using 90-theta for lat.
// So for theta use 85 degrees, in radians: 0.08726646259971647884618453842443
T const expected_equatorial_x = 0.962250;
T const expected_equatorial_y = 0.257834;
T const expected_equatorial_z = 0.0871557;
// 1: Spherical-polar (lat=5, so it is near the pole - on a unit sphere)
bg::model::point<T, 2, bg::cs::spherical<bg::degree> > sp(input_long, input_lat);
// 1a: to radian
bg::model::point<T, 2, bg::cs::spherical<bg::radian> > spr;
bg::transform(sp, spr);
BOOST_CHECK_CLOSE(bg::get<0>(spr), expected_long, 0.001);
BOOST_CHECK_CLOSE(bg::get<1>(spr), expected_lat, 0.001);
// 1b: to cartesian-3d
bg::model::point<T, 3, bg::cs::cartesian> pc3;
bg::transform(sp, pc3);
BOOST_CHECK_CLOSE(bg::get<0>(pc3), expected_polar_x, 0.001);
BOOST_CHECK_CLOSE(bg::get<1>(pc3), expected_polar_y, 0.001);
BOOST_CHECK_CLOSE(bg::get<2>(pc3), expected_polar_z, 0.001);
BOOST_CHECK_CLOSE(check_distance<T>(pc3), 1.0, 0.001);
// 1c: back
bg::transform(pc3, spr);
BOOST_CHECK_CLOSE(bg::get<0>(spr), expected_long, 0.001);
BOOST_CHECK_CLOSE(bg::get<1>(spr), expected_lat, 0.001);
// 2: Spherical-equatorial (lat=5, so it is near the equator)
bg::model::point<T, 2, bg::cs::spherical_equatorial<bg::degree> > se(input_long, input_lat);
// 2a: to radian
bg::model::point<T, 2, bg::cs::spherical_equatorial<bg::radian> > ser;
bg::transform(se, ser);
BOOST_CHECK_CLOSE(bg::get<0>(ser), expected_long, 0.001);
BOOST_CHECK_CLOSE(bg::get<1>(ser), expected_lat, 0.001);
bg::transform(se, pc3);
BOOST_CHECK_CLOSE(bg::get<0>(pc3), expected_equatorial_x, 0.001);
BOOST_CHECK_CLOSE(bg::get<1>(pc3), expected_equatorial_y, 0.001);
BOOST_CHECK_CLOSE(bg::get<2>(pc3), expected_equatorial_z, 0.001);
BOOST_CHECK_CLOSE(check_distance<T>(pc3), 1.0, 0.001);
// 2c: back
bg::transform(pc3, ser);
BOOST_CHECK_CLOSE(bg::get<0>(spr), expected_long, 0.001); // expected_long
BOOST_CHECK_CLOSE(bg::get<1>(spr), expected_lat, 0.001); // expected_lat
// 3: Spherical-polar including radius
bg::model::point<T, 3, bg::cs::spherical<bg::degree> > sp3(input_long, input_lat, 0.5);
// 3a: to radian
bg::model::point<T, 3, bg::cs::spherical<bg::radian> > spr3;
bg::transform(sp3, spr3);
BOOST_CHECK_CLOSE(bg::get<0>(spr3), expected_long, 0.001);
BOOST_CHECK_CLOSE(bg::get<1>(spr3), expected_lat, 0.001);
BOOST_CHECK_CLOSE(bg::get<2>(spr3), 0.5, 0.001);
// 3b: to cartesian-3d
bg::transform(sp3, pc3);
BOOST_CHECK_CLOSE(bg::get<0>(pc3), expected_polar_x / 2.0, 0.001);
BOOST_CHECK_CLOSE(bg::get<1>(pc3), expected_polar_y / 2.0, 0.001);
BOOST_CHECK_CLOSE(bg::get<2>(pc3), expected_polar_z / 2.0, 0.001);
BOOST_CHECK_CLOSE(check_distance<T>(pc3), 0.5, 0.001);
// 3c: back
bg::transform(pc3, spr3);
BOOST_CHECK_CLOSE(bg::get<0>(spr3), expected_long, 0.001);
BOOST_CHECK_CLOSE(bg::get<1>(spr3), expected_lat, 0.001);
BOOST_CHECK_CLOSE(bg::get<2>(spr3), 0.5, 0.001);
// 4: Spherical-equatorial including radius
bg::model::point<T, 3, bg::cs::spherical_equatorial<bg::degree> > se3(input_long, input_lat, 0.5);
// 4a: to radian
bg::model::point<T, 3, bg::cs::spherical_equatorial<bg::radian> > ser3;
bg::transform(se3, ser3);
BOOST_CHECK_CLOSE(bg::get<0>(ser3), expected_long, 0.001);
BOOST_CHECK_CLOSE(bg::get<1>(ser3), expected_lat, 0.001);
BOOST_CHECK_CLOSE(bg::get<2>(ser3), 0.5, 0.001);
// 4b: to cartesian-3d
bg::transform(se3, pc3);
BOOST_CHECK_CLOSE(bg::get<0>(pc3), expected_equatorial_x / 2.0, 0.001);
BOOST_CHECK_CLOSE(bg::get<1>(pc3), expected_equatorial_y / 2.0, 0.001);
BOOST_CHECK_CLOSE(bg::get<2>(pc3), expected_equatorial_z / 2.0, 0.001);
BOOST_CHECK_CLOSE(check_distance<T>(pc3), 0.5, 0.001);
// 4c: back
bg::transform(pc3, ser3);
BOOST_CHECK_CLOSE(bg::get<0>(ser3), expected_long, 0.001);
BOOST_CHECK_CLOSE(bg::get<1>(ser3), expected_lat, 0.001);
BOOST_CHECK_CLOSE(bg::get<2>(ser3), 0.5, 0.001);
}
// \delta v = (I - h J_v)^{-1} (h a)
// \delta q = h (\delta v + v)
void vpSystem::IntegrateDynamicsBackwardEulerFast(scalar h)
{
if ( m_pRoot->m_bIsGround )
{
int i, j, n = m_sState.size();
if ( !n ) return;
ForwardDynamics();
RMatrix _a(n,1), _v(n,1);
RMatrix _Jv(n,n);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
RMatrix delv, A = Eye<scalar>(n) - h * _Jv;
SolveAxEqualB(A, delv, h * _a);
RMatrix delq = h * (delv + _v);
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + delv[i]);
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + delq[i]);
}
} else
{
int i, j, n = m_sState.size(), m = n + 6;
ForwardDynamics();
RMatrix _a(m,1), _v(m,1);
RMatrix _Jv(m,m);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
memcpy(&_a[n], &m_pRoot->m_sDV[0], sizeof(se3));
memcpy(&_v[n], &m_pRoot->m_sV[0], sizeof(se3));
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
for ( i = 0; i < 6; i++ )
{
m_pRoot->m_sV[i] += LIE_EPS;
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_pRoot->m_sV[i] -= LIE_EPS;
}
RMatrix delv, A = Eye<scalar>(m) - h * _Jv;
SolveAxEqualB(A, delv, h * _a);
RMatrix delq = h * (delv + _v);
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + delv[i]);
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + delq[i]);
}
for ( i = 0; i < 6; i++ ) m_pRoot->m_sV[i] += delv[n+i];
m_pRoot->m_sFrame *= Exp(se3(delq[n], delq[n+1], delq[n+2], delq[n+3], delq[n+4], delq[n+5]));
}
Reparameterize();
}
// \delta v = (I - h J_v - h^2 J_q)^{-1} (h a + h^2 J_q v)
// \delta q = h (\delta v + v)
void vpSystem::IntegrateDynamicsBackwardEuler(scalar time_step)
{
if ( m_pRoot->m_bIsGround )
{
int i, j, n = m_sState.size();
if ( !n ) return;
ForwardDynamics();
RMatrix _a(n,1), _v(n,1);
RMatrix _Jq(n,n), _Jv(n,n);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() - LIE_EPS);
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
_Jq *= (time_step * time_step);
_Jv *= -time_step;
_Jv -= _Jq;
for ( i = 0; i < n; i++ ) _Jv[i*(n+1)] += SCALAR_1;
_a *= time_step;
_a += _Jq * _v;
SolveAxEqualB_(_Jv, _a);
for ( i = 0; i < n; i++ ) m_sState[i].SetVelocity(_v[i] + _a[i]);
_a += _v;
_a *= time_step;
for ( i = 0; i < n; i++ ) m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + _a[i]);
} else
{
int i, j, n = m_sState.size(), m = n + 6;
ForwardDynamics();
RMatrix _a(m,1), _v(m,1), _dx(m,1);
RMatrix _Jq(m,m), _Jv(m,m), _M = Eye<scalar>(m,m);
SE3 _T;
se3 gv(SCALAR_0);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
memcpy(&_a[n], &m_pRoot->m_sDV[0], sizeof(se3));
memcpy(&_v[n], &m_pRoot->m_sV[0], sizeof(se3));
for ( i = 0; i < n; i++ )
{
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jq(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() - LIE_EPS);
}
for ( i = 0; i < 6; i++ )
{
gv[i] += LIE_EPS;
_T = m_pRoot->m_sFrame;
m_pRoot->m_sFrame *= Exp(gv);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jq(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_pRoot->m_sFrame = _T;
gv[i] = SCALAR_0;
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
for ( i = 0; i < 6; i++ )
{
m_pRoot->m_sV[i] += LIE_EPS;
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_pRoot->m_sV[i] -= LIE_EPS;
}
_Jq *= (time_step * time_step);
_Jv *= time_step;
_M -= _Jq;
_M -= _Jv;
_a *= time_step;
_a += _Jq * _v;
SolveAxEqualB(_M, _dx, _a);
for ( i = 0; i < n; i++ ) m_sState[i].SetVelocity(m_sState[i].GetVelocity() + _dx[i]);
for ( i = 0; i < 6; i++ ) m_pRoot->m_sV[i] += _dx[n+i];
_dx += _v;
_dx *= time_step;
for ( i = 0; i < n; i++ ) m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + _dx[i]);
m_pRoot->m_sFrame *= Exp(se3(_dx[n], _dx[n+1], _dx[n+2], _dx[n+3], _dx[n+4], _dx[n+5]));
}
Reparameterize();
}