コード例 #1
0
ファイル: Skeleton.cpp プロジェクト: hsu/dart
void Skeleton::computeForwardDynamicsID2(
        const Eigen::Vector3d& _gravity, bool _equationsOfMotion)
{
    int n = getDOF();

    // skip immobile objects in forward simulation
    if (getImmobileState() == true || n == 0)
    {
        return;
    }

    // Save current tau
    Eigen::VectorXd tau_old = get_tau();

    // Set ddq as zero
    set_ddq(Eigen::VectorXd::Zero(n));

    //
    mM = Eigen::MatrixXd::Zero(n,n);

    // M(q) * ddq + b(q,dq) = tau
    computeInverseDynamicsLinear(_gravity);
    Eigen::VectorXd b = get_tau();
    mCg = b;

    // Calcualtion M column by column
    for (int i = 0; i < n; ++i)
    {
        Eigen::VectorXd basis = Eigen::VectorXd::Zero(n);
        basis(i) = 1;
        set_ddq(basis);
        computeInverseDynamicsLinear(_gravity);
        mM.col(i) = get_tau() - b;
    }

    // Restore the torque
    set_tau(tau_old);

    // Evaluate external forces in generalized coordinate.
    updateExternalForces();

    Eigen::VectorXd qddot = this->getInvMassMatrix()
                            * (-this->getCombinedVector()
                               + this->getExternalForces()
                               + this->getInternalForces()
                               + this->getDampingForces()
                               + this->getConstraintForces() );

    //mMInv = mM.inverse();
    mMInv = mM.ldlt().solve(Eigen::MatrixXd::Identity(n,n));

    this->set_ddq(qddot);

    clearExternalForces();
}
コード例 #2
0
ファイル: gsystem.cpp プロジェクト: mrdeveloperdude/OctoMY
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
ファイル: gsystem.cpp プロジェクト: mrdeveloperdude/OctoMY
void GSystem::getEquationsOfMotion(RMatrix &M, RMatrix &b)
{
	RMatrix ddq = get_ddq(), tau = get_tau(); // save current ddq and tau
	int n = getNumCoordinates();
	M.ReNew(n,n);
	set_ddq(Zeros(n,1));
	GSystem::calcInverseDynamics();
	b = get_tau();
	for (int i=0; i<n; i++) {
		RMatrix unit = Zeros(n,1);
		unit[i] = 1;
		set_ddq(unit);
		GSystem::calcInverseDynamics();
		get_tau(&M[i*n]);
		for (int j=0; j<n; j++) {
			M[i*n+j] -= b[j];
		}
	}
	set_ddq(ddq); set_tau(tau); // restore ddq and tau
}
コード例 #4
0
ファイル: Skeleton.cpp プロジェクト: hsu/dart
void Skeleton::computeEquationsOfMotionID(
        const Eigen::Vector3d& _gravity)
{
    int n = getDOF();

    // skip immobile objects in forward simulation
    if (getImmobileState() == true || n == 0)
    {
        return;
    }

    // Save current tau
    Eigen::VectorXd tau_old = get_tau();

    // Set ddq as zero
    set_ddq(Eigen::VectorXd::Zero(n));

    // M(q) * ddq + b(q,dq) = tau
    computeInverseDynamicsLinear(_gravity, true);
    mCg = get_tau();

    // Calcualtion mass matrix, M
    mM = Eigen::MatrixXd::Zero(n,n);
    for (int i = 0; i < getNumBodyNodes(); i++)
    {
        BodyNode *nodei = getBodyNode(i);
        nodei->updateMassMatrix();
        nodei->aggregateMass(mM);
    }

    // Inverse of mass matrix
    mMInv = mM.ldlt().solve(Eigen::MatrixXd::Identity(n,n));

    // Restore the torque
    set_tau(tau_old);

    // Evaluate external forces in generalized coordinate.
    updateExternalForces();

    // Update damping forces
    updateDampingForces();
}
コード例 #5
0
ファイル: Skeleton.cpp プロジェクト: hsu/dart
Eigen::VectorXd Skeleton::computeInverseDynamicsLinear(
        const Eigen::Vector3d& _gravity,
        const Eigen::VectorXd* _qdot,
        const Eigen::VectorXd* _qdotdot,
        bool _computeJacobians,
        bool _withExternalForces,
        bool _withDampingForces)
{
    int n = getDOF();

    if (_qdot == NULL)
        set_dq(Eigen::VectorXd::Zero(n));

    if (_qdotdot == NULL)
        set_ddq(Eigen::VectorXd::Zero(n));

    _updateJointKinematics();

    // Forward recursion
    for (std::vector<dynamics::BodyNode*>::iterator itrBody = mBodyNodes.begin();
         itrBody != mBodyNodes.end();
         ++itrBody) {
        (*itrBody)->updateTransform();
        (*itrBody)->updateVelocity(_computeJacobians);
        (*itrBody)->updateEta();
        (*itrBody)->updateAcceleration(_computeJacobians);
    }

    // Backward recursion
    for (std::vector<dynamics::BodyNode*>::reverse_iterator ritrBody
         = mBodyNodes.rbegin();
         ritrBody != mBodyNodes.rend();
         ++ritrBody)
    {
        (*ritrBody)->updateBodyForce(_gravity,
                                     _withExternalForces);
        (*ritrBody)->updateGeneralizedForce(_withDampingForces);
    }

    return get_tau();
}