void GenCoordSystem::set_dq(const Eigen::VectorXd& _dq)
{
assert(_dq.size() == getDOF());
int size = getDOF();
for (int i = 0; i < size; ++i)
mGenCoords[i]->set_dq(_dq[i]);
}
void GenCoordSystem::set_tauMax(const Eigen::VectorXd& _tauMax)
{
assert(_tauMax.size() == getDOF());
int size = getDOF();
for (int i = 0; i < size; ++i)
mGenCoords[i]->set_tauMax(_tauMax[i]);
}
void GenCoordSystem::set_qMin(const Eigen::VectorXd& _qMin)
{
assert(_qMin.size() == getDOF());
int size = getDOF();
for (int i = 0; i < size; ++i)
mGenCoords[i]->set_qMin(_qMin[i]);
}
void GenCoordSystem::restoreInitState()
{
int size = getDOF();
for (int i = 0; i < size; ++i)
mGenCoords[i]->restoreInitState();
}
void GenCoordSystem::backupInitState()
{
int size = getDOF();
for (int i = 0; i < size; ++i)
mGenCoords[i]->backupInitState();
}
GenCoord* GenCoordSystem::getGenCoord(const std::string& _name) const
{
int size = getDOF();
for (int i = 0; i < size; ++i)
if (mGenCoords[i]->getName() == _name)
return mGenCoords[i];
return NULL;
}
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();
}
Eigen::VectorXd GenCoordSystem::get_tauMin() const
{
int size = getDOF();
Eigen::VectorXd tau = Eigen::VectorXd::Zero(size);
for (int i = 0; i < size; ++i) {
tau(i) = mGenCoords[i]->get_tauMin();
}
return tau;
}
Eigen::VectorXd GenCoordSystem::get_ddqMin() const
{
int size = getDOF();
Eigen::VectorXd ddq = Eigen::VectorXd::Zero(size);
for (int i = 0; i < size; ++i) {
ddq(i) = mGenCoords[i]->get_ddqMin();
}
return ddq;
}
void Skeleton::setPose(const Eigen::VectorXd& _pose,
bool bCalcTrans,
bool bCalcDeriv)
{
for (int i = 0; i < getDOF(); i++)
mGenCoords.at(i)->set_q(_pose[i]);
if (bCalcTrans)
{
if (bCalcDeriv)
updateForwardKinematics(true, false);
else
updateForwardKinematics(false, false);
}
}
void Skeleton::computeForwardDynamicsFS(
const Eigen::Vector3d& _gravity, bool _equationsOfMotion)
{
int n = getDOF();
// skip immobile objects in forward simulation
if (getImmobileState() == true || n == 0)
{
return;
}
// Forward recursion
for (std::vector<dynamics::BodyNode*>::iterator itrBody = mBodyNodes.begin();
itrBody != mBodyNodes.end();
++itrBody)
{
(*itrBody)->updateTransform();
(*itrBody)->updateVelocity();
(*itrBody)->updateEta();
}
// Backward recursion
for (std::vector<dynamics::BodyNode*>::reverse_iterator ritrBody
= mBodyNodes.rbegin();
ritrBody != mBodyNodes.rend();
++ritrBody)
{
(*ritrBody)->updateArticulatedInertia();
(*ritrBody)->updateBiasForce(_gravity);
(*ritrBody)->updatePsi();
(*ritrBody)->updatePi();
(*ritrBody)->updateBeta();
}
for (std::vector<dynamics::BodyNode*>::iterator itrBody = mBodyNodes.begin();
itrBody != mBodyNodes.end();
++itrBody)
{
(*itrBody)->update_ddq();
(*itrBody)->updateAcceleration();
(*itrBody)->update_F_fs();
}
clearExternalForces();
}
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();
}
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();
}
void Skeleton::initDynamics()
{
initKinematics();
int DOF = getDOF();
mM = Eigen::MatrixXd::Zero(DOF, DOF);
mMInv = Eigen::MatrixXd::Zero(DOF, DOF);
mC = Eigen::MatrixXd::Zero(DOF, DOF);
mCvec = Eigen::VectorXd::Zero(DOF);
mG = Eigen::VectorXd::Zero(DOF);
mCg = Eigen::VectorXd::Zero(DOF);
set_tau(Eigen::VectorXd::Zero(DOF));
mFext = Eigen::VectorXd::Zero(DOF);
mFc = Eigen::VectorXd::Zero(DOF);
mDampingForce = Eigen::VectorXd::Zero(DOF);
// calculate mass
// init the dependsOnDof stucture for each bodylink
mTotalMass = 0.0;
for(int i = 0; i < getNumBodyNodes(); i++)
mTotalMass += getBodyNode(i)->getMass();
}
void Skeleton::clearInternalForces()
{
set_tau(Eigen::VectorXd::Zero(getDOF()));
}
GenCoord* GenCoordSystem::getGenCoord(int _idx) const
{
assert(0 <= _idx && _idx < getDOF());
return mGenCoords[_idx];
}
int getGlobalDOF(const int inode, const int eq) const
{
return getDOF(inode,eq);
}
int getOverlapDOF(const int inode, const int eq) const
{
return getDOF(inode,eq);
}
bool
RobotIQ::autoGrasp(bool renderIt, double speedFactor, bool stopAtContact)
{
//not implemented for the RobotIQ yet
if (myWorld->dynamicsAreOn()) return Hand::autoGrasp(renderIt, speedFactor, stopAtContact);
if (numDOF != 11) {DBGA("Hard-coded autograsp does not match RobotIQ hand"); return false;}
std::vector<double> desiredVals(11, 0.0);
if (speedFactor < 0)
{
DBGA("Hand opening not yet implemented for RobotIQ hand; forcing it to open pose");
desiredVals[3] = getDOF(3)->getVal();
desiredVals[7] = getDOF(7)->getVal();
forceDOFVals(&desiredVals[0]);
return false;
}
std::vector<double> desiredSteps(11, 0.0);
desiredVals[0] = desiredVals[4] = desiredVals[8] = dofVec[0]->getMax();
desiredVals[2] = desiredVals[6] = desiredVals[10] = dofVec[2]->getMin();
desiredSteps[0] = desiredSteps[4] = desiredSteps[8] = speedFactor*AUTO_GRASP_TIME_STEP;
desiredSteps[2] = desiredSteps[6] = desiredSteps[10] = -speedFactor*AUTO_GRASP_TIME_STEP;
int steps=0;
while(1)
{
bool moved = moveDOFToContacts(&desiredVals[0], &desiredSteps[0], true, renderIt);
std::vector<double> ref;
ref.push_back(0); ref.push_back(4); ref.push_back(8);
std::vector<double> links;
links.push_back(0); links.push_back(1); links.push_back(1);
for(size_t i=0; i<ref.size(); i++)
{
if (getChain(i)->getLink(links[i]+0)->getNumContacts() ||
getChain(i)->getLink(links[i]+1)->getNumContacts() ||
getDOF(ref[i]+0)->getVal() == getDOF(ref[i]+0)->getMax() )
{
desiredVals[ref[i]+0] = getDOF(ref[i]+0)->getVal();
desiredSteps[ref[i]+0] = 0;
if (!getChain(i)->getLink(links[i]+1)->getNumContacts())
{
desiredVals[ref[i]+1] = getDOF(ref[i]+1)->getMax();
desiredSteps[ref[i]+1] = speedFactor*AUTO_GRASP_TIME_STEP;
}
else
{
desiredVals[ref[i]+1] = getDOF(ref[i]+1)->getVal();
desiredSteps[ref[i]+1] = 0;
}
desiredVals[ref[i]+2] = getDOF(ref[i]+2)->getMax();
desiredSteps[ref[i]+2] = speedFactor*AUTO_GRASP_TIME_STEP;
}
}
if (!moved) break;
if (++steps > 1000)
{
DBGA("RobotIQ hard-coded autograsp seems to be looping forever; escaping...");
break;
}
}
return true;
}
int getOwnedDOF(const int inode, const int eq) const
{
return getDOF(inode,eq);
}
