JointIndex Model::addJoint(const std::string& jointName, IJointConstPtr joint)
{
assert(joint->getFirstAttachedLink() != joint->getSecondAttachedLink());
if(isJointNameUsed(jointName))
{
std::string error = "a joint of name " + jointName + " is already present in the model";
reportError("Model","addJoint",error.c_str());
return JOINT_INVALID_INDEX;
}
// Check that the joint is referring to links that are in the model
LinkIndex firstLink = joint->getFirstAttachedLink();
LinkIndex secondLink = joint->getSecondAttachedLink();
if( firstLink < 0 || firstLink >= (LinkIndex)this->getNrOfLinks() ||
secondLink < 0 || secondLink >= (LinkIndex)this->getNrOfLinks() )
{
std::string error = "joint " + jointName + " is attached to a link that does not exist";
reportError("Model","addJoint",error.c_str());
return JOINT_INVALID_INDEX;
}
// Check that the joint is not connecting a link to itself
if( firstLink == secondLink )
{
std::string error = "joint " + jointName + " is connecting link " + this->getLinkName(firstLink) + " to itself";
reportError("Model","addJoint",error.c_str());
return JOINT_INVALID_INDEX;
}
// Update the joints and jointNames structure
jointNames.push_back(jointName);
joints.push_back(joint->clone());
JointIndex thisJointIndex = (JointIndex)(joints.size()-1);
// Update the adjacency list
Neighbor firstLinkNeighbor;
firstLinkNeighbor.neighborLink = secondLink;
firstLinkNeighbor.neighborJoint = thisJointIndex;
this->neighbors[firstLink].push_back(firstLinkNeighbor);
Neighbor secondLinkNeighbor;
secondLinkNeighbor.neighborLink = firstLink;
secondLinkNeighbor.neighborJoint = thisJointIndex;
this->neighbors[secondLink].push_back(secondLinkNeighbor);
// Set the joint index and dof offset
this->joints[thisJointIndex]->setIndex(thisJointIndex);
this->joints[thisJointIndex]->setPosCoordsOffset(this->nrOfPosCoords);
this->joints[thisJointIndex]->setDOFsOffset(this->nrOfDOFs);
// Update the number of dofs
this->nrOfPosCoords += this->joints[thisJointIndex]->getNrOfPosCoords();
this->nrOfDOFs += this->joints[thisJointIndex]->getNrOfDOFs();
return thisJointIndex;
}
void copyFromFullToReduced( vectorType & reducedVector,
const vectorType & fullVector,
const Model & reducedModel,
const Model & fullModel)
{
for(JointIndex jntReduced = 0; jntReduced < reducedModel.getNrOfJoints(); jntReduced++ )
{
IJointConstPtr jnt = reducedModel.getJoint(jntReduced);
std::string jointName = reducedModel.getJointName(jntReduced);
if( jnt->getNrOfDOFs() > 0 )
{
IJointConstPtr jntInFullModel = fullModel.getJoint(fullModel.getJointIndex(jointName));
reducedVector(jnt->getPosCoordsOffset()) = fullVector(jntInFullModel->getPosCoordsOffset());
}
}
}
bool FreeFloatingJacobianUsingLinkPos(const Model& model,
const Traversal& traversal,
const JointPosDoubleArray& jointPositions,
const LinkPositions& world_H_links,
const LinkIndex jacobianLinkIndex,
const Transform& jacobFrame_X_world,
const Transform& baseFrame_X_jacobBaseFrame,
MatrixDynSize& jacobian)
{
// We zero the jacobian
jacobian.zero();
// Compute base part
const Transform & world_H_base = world_H_links(traversal.getBaseLink()->getIndex());
toEigen(jacobian).block(0,0,6,6) = toEigen((jacobFrame_X_world*world_H_base*baseFrame_X_jacobBaseFrame).asAdjointTransform());
// Compute joint part
// We iterate from the link up in the traveral until we reach the base
LinkIndex visitedLinkIdx = jacobianLinkIndex;
while (visitedLinkIdx != traversal.getBaseLink()->getIndex())
{
LinkIndex parentLinkIdx = traversal.getParentLinkFromLinkIndex(visitedLinkIdx)->getIndex();
IJointConstPtr joint = traversal.getParentJointFromLinkIndex(visitedLinkIdx);
size_t dofOffset = joint->getDOFsOffset();
for(int i=0; i < joint->getNrOfDOFs(); i++)
{
toEigen(jacobian).block(0,6+dofOffset+i,6,1) =
toEigen(jacobFrame_X_world*(world_H_links(visitedLinkIdx)*joint->getMotionSubspaceVector(i,visitedLinkIdx,parentLinkIdx)));
}
visitedLinkIdx = parentLinkIdx;
}
return true;
}
void copyFromReducedToFull(const vectorType & reducedVector,
vectorType & fullVector,
const Model & reducedModel,
const Model & fullModel)
{
for(JointIndex jntReduced = 0; jntReduced < reducedModel.getNrOfJoints(); jntReduced++ )
{
IJointConstPtr jnt = reducedModel.getJoint(jntReduced);
std::string jointName = reducedModel.getJointName(jntReduced);
if( jnt->getNrOfDOFs() > 0 )
{
IJointConstPtr jntInFullModel = fullModel.getJoint(fullModel.getJointIndex(jointName));
assert(jntInFullModel->getNrOfDOFs() > 0);
assert(fullVector.size() == fullModel.getNrOfPosCoords());
assert(reducedVector.size() == reducedModel.getNrOfPosCoords());
assert(jntInFullModel->getPosCoordsOffset() < fullVector.size());
assert(jnt->getPosCoordsOffset() < reducedVector.size());
fullVector(jntInFullModel->getPosCoordsOffset()) = reducedVector(jnt->getPosCoordsOffset());
}
}
}
