コード例 #1
0
ファイル: Model.cpp プロジェクト: PerryZh/idyntree
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;
}
コード例 #2
0
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());
        }
    }
}
コード例 #3
0
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;
}
コード例 #4
0
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());
        }
    }
}