Пример #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;
}
Пример #2
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;
}
Пример #3
0
bool GSpringDamperJoint::setSpring(RMatrix K_)
{
	if ( K_.RowSize() * K_.ColSize() != pJoint->getDOF() ) return false;
	return setSpring(K_.GetPtr());
}
Пример #4
0
bool GSpringDamperJoint::setDamper(RMatrix C_)
{
	if ( C_.RowSize() * C_.ColSize() != pJoint->getDOF() ) return false;
	return setDamper(C_.GetPtr());
}
Пример #5
0
bool GSpringDamperJoint::setNeutralPosition(RMatrix q_neutral_)
{
	if ( q_neutral_.RowSize() * q_neutral_.ColSize() != pJoint->getDOF() ) return false;
	return setNeutralPosition(q_neutral_.GetPtr());
}