Esempio n. 1
0
bool GSystem::calcProductOfInvMassAndMatrix2(RMatrix &invM_A, const RMatrix &A)
{
	if ( (size_t)A.RowSize() != pCoordinates.size() ) return false;
	invM_A.SetZero(A.RowSize(), A.ColSize());

	int i;
	std::list<GJoint *>::iterator iter_pjoint;
	std::vector<bool> isprescribed(pJoints.size());

	// save current info
	for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
		isprescribed[i] = (*iter_pjoint)->isPrescribed();
	}

	setAllJointsPrescribed(false);

	initDynamicsWithZeroGravityAndVelocity();
	for (i=0; i<A.ColSize(); i++) {
		set_tau(&(A[i*A.RowSize()]));
		calcDynamicsWithZeroGravityAndVelocity();
		get_ddq(&(invM_A[i*invM_A.RowSize()]));
	}

	// restore
	for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
		(*iter_pjoint)->setPrescribed(isprescribed[i]);
	}

	return true;
}
Esempio n. 2
0
void GSystem::calcDerivative_PositionCOMGlobal_Dq(RMatrix &DpDq_)
{
	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());
	Vec3 DpDqi;

	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_PositionCOMGlobal_Dq(...)
		(*iter_pbody)->fwdJointChain.setM(SE3());
		(*iter_pbody)->fwdJointChain.jointLoopConstraintType = GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION;
		(*iter_pbody)->fwdJointChain.update_J();
	}

	// calculate the derivative
	DpDq_.SetZero(3, getNumCoordinates());
	for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
		DpDqi = getDerivative_PositionCOMGlobal_Dq(*iter_pcoord);
		//DpDq_.Push(0, i, convert_to_RMatrix(DpDqi));
		matSet(&DpDq_[3*i], DpDqi.GetArray(), 3);
	}

	// 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]);
	}
}
Esempio n. 3
0
bool GSystemIK::solveIK_dq(RMatrix &dq, std::vector<GBody*> pbodies_primary, std::vector<GBody*> pbodies_secondary, std::vector<Vec3> p_primary, std::vector<Vec3> p_secondary, std::vector<se3> V_primary, std::vector<se3> V_secondary, std::vector< std::vector<int> > idxC_primary, std::vector< std::vector<int> > idxC_secondary, gReal alpha_primary, gReal alpha_secondary)
{
	RMatrix Jp, Js;		// Jacobian matrices for primary/secondary constraints
	RMatrix Vp, Vs;		// the righthand side of the constraints

	if ( !buildConstrIK_dq(Jp, Vp, pbodies_primary, p_primary, V_primary, idxC_primary) ) return false;
	if ( !buildConstrIK_dq(Js, Vs, pbodies_secondary, p_secondary, V_secondary, idxC_secondary) ) return false;

	dq.SetZero(getNumCoordinates(), 1);
	if ( Jp.RowSize() > 0 ) {
		RMatrix dq0, N;
		dq0 = srInv(Jp, N, alpha_primary) * Vp;
		if ( Js.RowSize() > 0 ) {
			dq = dq0 + N * ( srInv(Js * N, alpha_secondary) * (Vs - Js * dq0) );
		} else {
			dq = dq0;
		}
	} else {
		if ( Js.RowSize() > 0 ) {
			dq = srInv(Js, alpha_secondary) * Vs;
		}
	}

	return true;
}
Esempio n. 4
0
bool GSystem::calcProductOfInvMassAndMatrix(RMatrix &invM_A, const RMatrix &A)
{
	if ( (size_t)A.RowSize() != pCoordinates.size() ) return false;
	invM_A.SetZero(A.RowSize(), A.ColSize());

	int i;
	std::list<GJoint *>::iterator iter_pjoint;
	std::vector<bool> isprescribed(pJoints.size());

	// save current info
	for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
		isprescribed[i] = (*iter_pjoint)->isPrescribed();
	}
	Vec3 g = getGravity();

	// set all joint unprescribed and set zero gravity
	setAllJointsPrescribed(false);
	setGravity(Vec3(0,0,0));

	update_joint_local_info_short();

	for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
		(*iter_pbody)->update_base_joint_info();
		(*iter_pbody)->update_T();
		(*iter_pbody)->set_eta_zero();
	}

	for (std::list<GBody *>::reverse_iterator riter_pbody = pBodies.rbegin(); riter_pbody != pBodies.rend(); riter_pbody++) {
		(*riter_pbody)->update_aI();
		(*riter_pbody)->update_Psi();
		(*riter_pbody)->update_Pi();
	}

	for (i=0; i<A.ColSize(); i++) {
		set_ddq(Zeros(pCoordinates.size(),1)); // this isn't necessary for real tree structure systems, but works for the cut joints in closed-loop
		set_tau(&(A[i*A.RowSize()]));

		for (std::list<GBody *>::reverse_iterator riter_pbody = pBodies.rbegin(); riter_pbody != pBodies.rend(); riter_pbody++) {
			(*riter_pbody)->update_aB_zeroV_zeroeta();
			(*riter_pbody)->update_beta_zeroeta();
		}

		for (std::list<GBody *>::iterator iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); iter_pbody++) {
			(*iter_pbody)->update_ddq();
			(*iter_pbody)->update_dV(true);
		}

		get_ddq(&(invM_A[i*invM_A.RowSize()]));
	}

	// restore
	for (i=0, iter_pjoint = pJoints.begin(); iter_pjoint != pJoints.end(); i++, iter_pjoint++) {
		(*iter_pjoint)->setPrescribed(isprescribed[i]);
	}
	setGravity(g);

	return true;
}
Esempio n. 5
0
void GSystem::calcDerivative_MomentumGlobal_Dq_Ddq(std::vector<GCoordinate*> pCoordinates_, RMatrix &DHgDq_, RMatrix &DHgDdq_)
{
	int i;
	std::list<GBody *>::iterator iter_pbody;
	std::vector<GCoordinate *>::iterator iter_pcoord;
	std::vector<SE3> M(pBodies.size());
	std::vector<GConstraintJointLoop::JOINTLOOP_CONSTRAINT_TYPE> jlc_type(pBodies.size());
	dse3 DHDqi, DHDdqi;

	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
	DHgDq_.SetZero(6, int(pCoordinates_.size()));
	DHgDdq_.SetZero(6, int(pCoordinates_.size()));
	for (i=0, iter_pcoord = pCoordinates_.begin(); iter_pcoord != pCoordinates_.end(); i++, iter_pcoord++) {
		DHDqi = getDerivative_MomentumGlobal_Dq(*iter_pcoord);
		DHDdqi = getDerivative_MomentumGlobal_Ddq(*iter_pcoord);
		//DHgDq_.Push(0, i, convert_to_RMatrix(DHDqi));
		//DHgDdq_.Push(0, i, convert_to_RMatrix(DHDdqi));
		matSet(&DHgDq_[6*i], DHDqi.GetArray(), 6);
		matSet(&DHgDdq_[6*i], DHDdqi.GetArray(), 6);
	}

	// 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]);
	}
}
Esempio n. 6
0
void GSystem::calcDerivative_PositionCOMGlobal_Dq_2(RMatrix &DpDq_)
{
	int i;
	std::list<GCoordinate *>::iterator iter_pcoord;
	Vec3 DpDqi;

	DpDq_.SetZero(3, getNumCoordinates());
	for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
		DpDqi = getDerivative_PositionCOMGlobal_Dq_2(*iter_pcoord);
		DpDq_(0,i) = DpDqi[0];
		DpDq_(1,i) = DpDqi[1];
		DpDq_(2,i) = DpDqi[2];
	}
}
Esempio n. 7
0
bool GSystemIK::buildConstrIK_dq(RMatrix &J, RMatrix &V, std::vector<GBody*> pbodies, std::vector<Vec3> pos, std::vector<se3> V_target, std::vector< std::vector<int> > idxC)
{
	int i, j, k;
	int cnt;			// a counter
	int nb;				// number of bodies
	int ncik;			// number of IK constraints
	std::list<GCoordinate*>::iterator iter_pcoord, iter_pcoord2;

	nb = int(pbodies.size());

	if ( pos.size() != (size_t)nb || V_target.size() != (size_t)nb || idxC.size() != (size_t)nb ) return false;

	// counts the number of IK constraints
	ncik = 0;
	for (i=0; i<nb; i++) {
		for ( j=0; j<int(idxC[i].size()); j++) {
			if ( idxC[i][j] < 0 || idxC[i][j] > 5 ) return false;
		}
		ncik += int(idxC[i].size());
	}
	J.SetZero(ncik, getNumCoordinates());
	V.SetZero(ncik, 1);

	// build J, V
	cnt = 0;
	for (i=0; i<nb; i++) {
		// update Jacobian
		pbodies[i]->fwdJointChain.setM(SE3(pos[i]));
		pbodies[i]->fwdJointChain.setJointLoopConstraintType(GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION);
		pbodies[i]->fwdJointChain.update_J();

		// transformed Jacobian, Jg = [R 0; 0 R] * J
		RMatrix Jg(pbodies[i]->fwdJointChain.J.RowSize(), pbodies[i]->fwdJointChain.J.ColSize());
		//RMatrix R = convert_to_RMatrix(pbodies[i]->T_global.GetRotation());
		RMatrix R(3,3); matSet(R.GetPtr(), pbodies[i]->T_global.GetRotation().GetArray(), 9);
		Jg.Push(0, 0, R * pbodies[i]->fwdJointChain.J.Sub(0, 2, 0, pbodies[i]->fwdJointChain.J.ColSize()-1));
		Jg.Push(3, 0, R * pbodies[i]->fwdJointChain.J.Sub(3, 5, 0, pbodies[i]->fwdJointChain.J.ColSize()-1));

		// build J
		for (j=0, iter_pcoord = pbodies[i]->fwdJointChain.pCoordinates.begin(); iter_pcoord != pbodies[i]->fwdJointChain.pCoordinates.end(); j++, iter_pcoord++) {

			// find index of pbodies[i]->fwdJointChain.pCoordinates[j] in pCoordinates
			int idx = -1;
			for (k=0, iter_pcoord2 = pCoordinates.begin(); iter_pcoord2 != pCoordinates.end(); k++, iter_pcoord2++) {
				if ( *iter_pcoord2 == *iter_pcoord ) { idx = k; break; }
			}
			if ( idx < 0 ) return false;

			// insert j-th column of the body Jacobian to the right place
			for (k=0; k<int(idxC[i].size()); k++) {
				J(cnt+k, idx) = Jg(idxC[i][k], j);
			}
		}

		// build V
		for (j=0; j<int(idxC[i].size()); j++) {
			V(cnt+j, 0) = V_target[i][idxC[i][j]];
		}

		cnt += int(idxC[i].size());
	}

	return true;
}
Esempio n. 8
0
bool GSystemIK::solveIK_dq(RMatrix &dq, std::vector<GBody*> pbodies_primary, std::vector<GBody*> pbodies_secondary, std::vector<Vec3> p_primary, std::vector<Vec3> p_secondary, std::vector<se3> V_primary, std::vector<se3> V_secondary, std::vector< std::vector<int> > idxC_primary, std::vector< std::vector<int> > idxC_secondary, std::vector<GCoordinate*> pcoords_prescribed, std::ofstream *pfout, gReal alpha_primary, gReal alpha_secondary)
{
	if ( pcoords_prescribed.size() == 0 ) {
		return solveIK_dq(dq, pbodies_primary, pbodies_secondary, p_primary, p_secondary, V_primary, V_secondary, idxC_primary, idxC_secondary, alpha_primary, alpha_secondary);
	}

	std::vector<GCoordinate*>::iterator viter_pcoord;
	std::list<GCoordinate*>::iterator iter_pcoord;
	RMatrix Jp, Js;		// Jacobian matrices for primary/secondary constraints
	RMatrix Vp, Vs;		// the righthand isde of the constraints

	if ( !buildConstrIK_dq(Jp, Vp, pbodies_primary, p_primary, V_primary, idxC_primary) ) return false;
	if ( !buildConstrIK_dq(Js, Vs, pbodies_secondary, p_secondary, V_secondary, idxC_secondary) ) return false;

	int num_prescribed = 0;
	std::vector<bool> b_prescribed;
	for (iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); iter_pcoord++) {
		if ( find(pcoords_prescribed.begin(), pcoords_prescribed.end(), *iter_pcoord) == pcoords_prescribed.end() ) {
			b_prescribed.push_back(false);
		} else {
			b_prescribed.push_back(true);
			num_prescribed++;
		}
	}

	std::vector<GCoordinate*> pcoords_all(pCoordinates.begin(), pCoordinates.end());
	int nx = getNumCoordinates() - num_prescribed;
	RMatrix Jp_(Jp.RowSize(), nx), Js_(Js.RowSize(), nx);
	RMatrix Vp_(Vp), Vs_(Vs);

	int cnt = 0;
	for (int i=0; i<getNumCoordinates(); i++) {
		if ( b_prescribed[i] ) {
			Vp_ -= pcoords_all[i]->dq * Jp.Sub(0, Jp.RowSize()-1, i, i);
			Vs_ -= pcoords_all[i]->dq * Js.Sub(0, Js.RowSize()-1, i, i);
		} else {
			Jp_.Push(0, cnt, Jp.Sub(0, Jp.RowSize()-1, i, i));
			Js_.Push(0, cnt, Js.Sub(0, Js.RowSize()-1, i, i));
			cnt++;
		}
	}

	RMatrix x;
	x.SetZero(nx,1);
	if ( Jp_.RowSize() > 0 ) {
		RMatrix x0, N;
		x0 = srInv(Jp_, N, alpha_primary) * Vp_;
		if ( Js_.RowSize() > 0 ) {
			x = x0 + N * ( srInv(Js_ * N, alpha_secondary) * (Vs_ - Js_ * x0) );
		} else {
			x = x0;
		}
	} else {
		if ( Js_.RowSize() > 0 ) {
			x = srInv(Js_, alpha_secondary) * Vs_;
		}
	}

	dq.ReSize(getNumCoordinates(), 1);
	cnt = 0;
	for (int i=0; i<getNumCoordinates(); i++) {
		if ( b_prescribed[i] ) {
			dq[i] = pcoords_all[i]->dq;
		} else {
			dq[i] = x[cnt++];
		}
	}

	if ( pfout != NULL ) {
		*pfout << "x = " << x << "Jp_ = " << Jp_ << "Js_ = " << Js_ << "Vp_ = " << Vp_ << "Vs_ = " << Vs_;
		*pfout << "Jp = " << Jp << "Js = " << Js << "Vp = " << Vp << "Vs = " << Vs;
		*pfout << "dq_prescribed = (";
		for (int i=0; i<(int)pcoords_prescribed.size(); i++) {
			*pfout << pcoords_prescribed[i]->dq << ", ";
		}
		*pfout << std::endl;
	}

	return true;
}