Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
bool GSystem::setGravity(Vec3 g_)
{
	if ( pGround == NULL ) return false;

	pGround->dV = se3(Vec3(0,0,0),-g_);

	return true;
}
Exemplo n.º 3
0
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];
	}
}
Exemplo n.º 4
0
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;
	}
}
Exemplo n.º 5
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;	
}
Exemplo n.º 7
0
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]);
	}
}
Exemplo n.º 8
0
//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;
	}
}
Exemplo n.º 10
0
void vpSiloWorld::ReleaseRoof(void)
{
	roof.SetFrame(Vec3(0, 0, NUM_SILO_LEVEL + 10));
	roof.SetGenVelocity(se3(0));
}
Exemplo n.º 11
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]);
		}
	}
}
Exemplo n.º 12
0
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);
}
Exemplo n.º 13
0
// \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();
}
Exemplo n.º 14
0
// \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();
}