bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const
{
    // clear chain
    chain = Chain();

    // walk down from chain_root and chain_tip to the root of the tree
    vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip;
    for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s=s->second.parent){
        parents_chain_root.push_back(s->first);
        if (s->first == root_name) break;
    }
    if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false;
    for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s=s->second.parent){
        parents_chain_tip.push_back(s->first);
        if (s->first == root_name) break;
    }
    if (parents_chain_tip.empty() || parents_chain_tip.back()  != root_name) return false;

    // remove common part of segment lists
    SegmentMap::key_type last_segment = root_name;
    while (!parents_chain_root.empty() && !parents_chain_tip.empty() &&
           parents_chain_root.back() == parents_chain_tip.back()){
        last_segment = parents_chain_root.back();
        parents_chain_root.pop_back();
        parents_chain_tip.pop_back();
    }
    parents_chain_root.push_back(last_segment);


    // add the segments from the root to the common frame
    for (unsigned int s=0; s<parents_chain_root.size()-1; s++){
        Segment seg = getSegment(parents_chain_root[s])->second.segment;
        Frame f_tip = seg.pose(0.0).Inverse();
        Joint jnt = seg.getJoint();
        if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis)
            jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::RotAxis);
        else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis)
            jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::TransAxis);
        chain.addSegment(Segment(getSegment(parents_chain_root[s+1])->second.segment.getName(),
                                 jnt, f_tip, getSegment(parents_chain_root[s+1])->second.segment.getInertia()));
    }

    // add the segments from the common frame to the tip frame
    for (int s=parents_chain_tip.size()-1; s>-1; s--){
        chain.addSegment(getSegment(parents_chain_tip[s])->second.segment);
    }
    return true;
}
示例#2
0
void check_self_consistency(SkeletonPtr skeleton)
{
  for(size_t i=0; i<skeleton->getNumBodyNodes(); ++i)
  {
    BodyNode* bn = skeleton->getBodyNode(i);
    EXPECT_TRUE(bn->getIndexInSkeleton() == i);
    EXPECT_TRUE(skeleton->getBodyNode(bn->getName()) == bn);

    Joint* joint = bn->getParentJoint();
    EXPECT_TRUE(skeleton->getJoint(joint->getName()) == joint);

    for(size_t j=0; j<joint->getNumDofs(); ++j)
    {
      DegreeOfFreedom* dof = joint->getDof(j);
      EXPECT_TRUE(dof->getIndexInJoint() == j);
      EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
    }
  }

  for(size_t i=0; i<skeleton->getNumDofs(); ++i)
  {
    DegreeOfFreedom* dof = skeleton->getDof(i);
    EXPECT_TRUE(dof->getIndexInSkeleton() == i);
    EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
  }
}
/**
 * Perform set up functions after model has been deserialized or copied.
 *
 * @param aEngine dynamics engine containing this dof.
 * @param aJoint joint containing this dof.
 */
void TransformAxis::connectToJoint(const Joint& aJoint)
{
    string errorMessage;

    _joint = &aJoint;

    // Look up the coordinates by name.
    const Property<string>& coordNames = getProperty_coordinates();
    int nc = coordNames.size();
    const auto& coords = _joint->getProperty_coordinates();

    if (!hasFunction()) {
        SimTK_ASSERT2_ALWAYS(coordNames.size() == 0,
            "CustomJoint (%s) %s axis has no function but has coordinates.",
            _joint->getName().c_str(), getName().c_str());
        return;
    }

    for(int i=0; i< nc; ++i) {
        if (coords.findIndexForName( coordNames[i] ) < 0) {
            errorMessage += "Invalid coordinate (" 
                            + coordNames[i] 
                            + ") specified for TransformAxis " 
                            + getName() + " in joint " + aJoint.getName();
            throw (Exception(errorMessage));
        }
    }
}
/**
	HACK!
*/
void Character::updateJointOrdering(){
	if( jointOrder.size() == joints.size() ) 
		return; // HACK assume ordering is ok
	jointOrder.clear();

	if (!root)
		return;
	DynamicArray<ArticulatedRigidBody*> bodies;
	bodies.push_back(root);

	int currentBody = 0;

	while ((uint)currentBody<bodies.size()){
		//add all the children joints to the list
		for (int i=0;i<bodies[currentBody]->getChildJointCount();i++){
			Joint *j = bodies[currentBody]->getChildJoint(i);
			jointOrder.push_back( getJointIndex(j->getName()) );
			bodies.push_back(bodies[currentBody]->getChildJoint(i)->getChild());
		}
		currentBody++;
	}

	reverseJointOrder.resize( jointOrder.size() );
	for( uint i=0; i < jointOrder.size(); ++i )
		reverseJointOrder[jointOrder[i]] = i;

}
示例#5
0
void Skeleton::insertJoint(string parentJointName, Joint& newJoint) {

	_jointsNames.push_back(QString(newJoint.getName().c_str()));
	_joints[newJoint.getName()] = newJoint;

	//--- Inserting non-root, update children list from parent joint
	if (!parentJointName.empty()) {

		_joints[newJoint.getName()].setParentJointName(parentJointName);
		_joints[parentJointName].insertChildJointName(newJoint.getName());

	} else {
		//--- Just one root Joint
		_rootJointName = newJoint.getName();

	}

}
void PlaneRegistration::init_kdl_robot()
{
  std::string urdf;
  node_->param("/robot_description", urdf, std::string());
  
  urdf::Model urdf_model;
  urdf_model.initString(urdf);
  
  // get tree from urdf string
  KDL::Tree my_tree;
  if (!kdl_parser::treeFromString(urdf, my_tree)) {
    ROS_ERROR("Failed to construct kdl tree");
    return;
  }   
  std::string rootLink = "wam/base_link";
  std::string tipLink = "wam/cutter_tip_link";
  if (!my_tree.getChain(rootLink, tipLink, robot_))
    {   
      ROS_ERROR("Failed to get chain from kdl tree, check root/rip link");
      return;
    }   
  num_jnts_ = robot_.getNrOfJoints();
  
  // resize joint states
  jnt_pos_.resize(num_jnts_);
  jnt_vel_.resize(num_jnts_);
  //    jnt_cmd_.resize(num_jnts_);
  jnt_limit_min_.resize(num_jnts_);
  jnt_limit_max_.resize(num_jnts_);
  
  // get jnt limits from URDF model
  size_t i_jnt = 0;
  for (size_t i = 0; i < robot_.getNrOfSegments(); i++) {
    Joint jnt = robot_.getSegment(i).getJoint();
    if (jnt.getType() != Joint::None) {
      jnt_limit_min_(i_jnt) = urdf_model.joints_[jnt.getName()]->limits->lower;
      jnt_limit_max_(i_jnt) = urdf_model.joints_[jnt.getName()]->limits->upper;
      i_jnt++;
    }
  }       
  
  // print limit for debugging
  std::cout << "min: " << jnt_limit_min_.data.transpose() << std::endl;
  std::cout << "max: " << jnt_limit_max_.data.transpose() << std::endl;
  
  // KDL Solvers
  fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(robot_));
  ik_solver_.reset(
		   new KDL::ChainIkSolverPos_LMA(
						 robot_,
						 1E-5,
						 500,
						 1E-15));
}
示例#7
0
文件: Skeleton.cpp 项目: hsu/dart
int Skeleton::getJointIndex(const std::string& _name) const
{
    const int nJoints = getNumJoints();

    for(int i = 0; i < nJoints; i++)
    {
        Joint* node = getJoint(i);

        if (_name == node->getName())
            return i;
    }

    return -1;
}
示例#8
0
//==============================================================================
void ReferentialSkeleton::unregisterJoint(BodyNode* _child)
{
  if(nullptr == _child)
  {
    dterr << "[ReferentialSkeleton::unregisterJoint] Attempting to unregister "
          << "a Joint from a nullptr BodyNode. This is most likely a bug. "
          << "Please report this!\n";
    assert(false);
    return;
  }

  Joint* joint = _child->getParentJoint();

  std::unordered_map<const BodyNode*, IndexMap>::iterator it =
      mIndexMap.find(_child);

  if( it == mIndexMap.end() || INVALID_INDEX == it->second.mJointIndex)
  {
    dterr << "[ReferentialSkeleton::unregisterJoint] Attempting to unregister "
          << "a Joint named [" << joint->getName() << "] (" << joint << "), "
          << "which is the parent Joint of BodyNode [" << _child->getName()
          << "] (" << _child << "), but the Joint is not currently in this "
          << "ReferentialSkeleton! This is most likely a bug. Please report "
          << "this!\n";
    assert(false);
    return;
  }

  std::size_t jointIndex = it->second.mJointIndex;
  mJoints.erase(mJoints.begin() + jointIndex);
  it->second.mJointIndex = INVALID_INDEX;

  for(std::size_t i = jointIndex; i < mJoints.size(); ++i)
  {
    // Re-index all of the Joints in this ReferentialSkeleton which came after
    // the Joint that was removed.
    JointPtr alteredJoint = mJoints[i];
    IndexMap& indexing = mIndexMap[alteredJoint.getBodyNodePtr()];
    indexing.mJointIndex = i;
  }

  if(it->second.isExpired())
    mIndexMap.erase(it);

  // Updating the caches isn't necessary after unregistering a joint right now,
  // but it might matter in the future, so it might be better to be safe than
  // sorry.
  updateCaches();
}
示例#9
0
Joint * BlueprintInstance::detachJoint(unsigned int i)
{
    assert(i < mJointList.size());
    Joint * detached = mJointList[i];

    // detach from list
    mJointList.erase(mJointList.begin() + i);

    // detach from map
    map<string, Joint*>::iterator iter =
        mJointMap.find(detached->getName());
    mJointMap.erase(iter);

    return detached;
}
示例#10
0
/* normal construction test */
void JointTest::normalConstruction() {

    int id = 1;
    std::string name = "test";
    float mass = 2;
    float length = 3;

    using namespace RoboticArm;

    Joint createdPart = Joint(id, name, mass, length);

    if (createdPart.getId() == id && createdPart.getName() == name &&
            createdPart.getLength() == length && createdPart.getMass() == mass) {
        CPPUNIT_ASSERT(true);
    } else {
        CPPUNIT_ASSERT(false);
    }

}
示例#11
0
void JointTest::validParReadBack(){
    
    bool success = true;

    int id = 111;
    std::string name = "test";
    float mass = 123.54566;
    float length = 14676.54;

    using namespace RoboticArm;

    try {
        Joint createdPart = Joint(id, name, mass, length);

        if (id != createdPart.getId()) {
            std::cout << "ID is wrong." << std::endl;
            success = false;
        }

        if (name != createdPart.getName()) {
            std::cout << "Name is wrong." << std::endl;
            success = false;
        }

        if (mass != createdPart.getMass()) {
            std::cout << "Mass is wrong." << std::endl;
            success = false;
        }

        if (length != createdPart.getLength()) {
            std::cout << "Length is wrong." << std::endl;
            success = false;
        }


    } catch (const std::invalid_argument& ia) {
        success = false;
    }

    CPPUNIT_ASSERT(success);

}
示例#12
0
//---Preorder
void Skeleton::printJoint(Joint& joint, int depth) {
	string logOutput = "";

	for (int i = 0; i < depth; i++) {
		logOutput += "\t";
	}
	logOutput += joint.getName();
	logOutput += "\n";
	for (int i = 0; i < depth; i++) {
		logOutput += "\t";
	}
	logDEBUG("%s(%d,%d,%d)",logOutput.c_str());

	vector<string> childrenJointsNames = joint.getChildrenJointsNames();

	for (unsigned int i = 0; i < childrenJointsNames.size(); i++) {
		printJoint(getJoint(childrenJointsNames.at(i)), depth + 1);
	}

}
示例#13
0
文件: IO.cpp 项目: BruceJawn/animata
/**
 * Creates skeleton object in XML.
 **/
void IO::saveSkeleton(TiXmlElement *parent, Skeleton *s, Mesh *m)
{
	vector<Joint *> *joints = s->getJoints();
	vector<Bone *> *bones = s->getBones();

	if (joints->empty() && bones->empty())
		return;

	TiXmlElement *skeletonXML = new TiXmlElement("skeleton");
	parent->LinkEndChild(skeletonXML);

	if (!joints->empty())
	{
		TiXmlElement *jointsXML = new TiXmlElement("joints");
		skeletonXML->LinkEndChild(jointsXML);

		vector<Joint *>::iterator i = joints->begin();
		for(; i < joints->end(); i++)
		{
			Joint *j = *i;
			TiXmlElement *jointXML = new TiXmlElement("joint");
			const char *name = j->getName();
			if (name[0] != 0) // save name only for named joints
				jointXML->SetAttribute("name", name);
			jointXML->SetDoubleAttribute("x", j->x);
			jointXML->SetDoubleAttribute("y", j->y);
			jointXML->SetAttribute("fixed", j->fixed);
			jointXML->SetAttribute("selected", j->selected);
			jointXML->SetAttribute("osc", j->osc);

			jointsXML->LinkEndChild(jointXML);
		}
	}

	if (!bones->empty())
	{
		saveBones(skeletonXML, bones, joints, m->getVertices());
	}
}
示例#14
0
//==============================================================================
void Controller::printDebugInfo() const
{
  std::cout << "[ATLAS Robot]" << std::endl
            << " NUM NODES : " << mAtlasRobot->getNumBodyNodes() << std::endl
            << " NUM DOF   : " << mAtlasRobot->getNumDofs() << std::endl
            << " NUM JOINTS: " << mAtlasRobot->getNumBodyNodes() << std::endl;

  for (std::size_t i = 0; i < mAtlasRobot->getNumBodyNodes(); ++i)
  {
    Joint* joint = mAtlasRobot->getJoint(i);
    BodyNode* body = mAtlasRobot->getBodyNode(i);
    BodyNode* parentBody = mAtlasRobot->getBodyNode(i)->getParentBodyNode();

    std::cout << "  Joint [" << i << "]: " << joint->getName() << " ("
              << joint->getNumDofs() << ")" << std::endl;
    if (parentBody != nullptr)
    {
      std::cout << "    Parent body: " << parentBody->getName() << std::endl;
    }

    std::cout << "    Child body : " << body->getName() << std::endl;
  }
}
COLLADASW::Joint KDLColladaLibraryJointsExporter::makeColladaSWJoint(COLLADASW::StreamWriter* streamWriter, Joint& kdlJoint, string uniqueId)
{


    string jointName = kdlJoint.getName();

    if (jointName == kdlDefaultJointName)
        jointName = uniqueId;
    Joint::JointType jointType = kdlJoint.getType();
    Vector jointAxis = kdlJoint.JointAxis();


    COLLADAFW::JointPrimitive::Type type;
    switch (jointType)
    {
        case Joint::RotAxis :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }
        case Joint::RotX :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }
        case Joint::RotY :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }
        case Joint::RotZ :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }

        case Joint::TransAxis :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        case Joint::TransX :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        case Joint::TransY :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        case Joint::TransZ :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        default :
            LOG(ERROR) << "Unknown joint type: " << kdlJoint.getTypeName() << ", changing to revolute type with no axis.";
            type = COLLADAFW::JointPrimitive::REVOLUTE;
    }

    COLLADABU::Math::Vector3 axis(jointAxis.x(), jointAxis.y(), jointAxis.z());

    COLLADAFW::JointPrimitive jointPrimitive(COLLADAFW::UniqueId(uniqueId), type);
    jointPrimitive.setAxis(axis);

   // jointPrimitive.setHardLimitMax(10);
   // jointPrimitive.setHardLimitMin(-10);


    COLLADASW::Joint colladaJoint(streamWriter, jointPrimitive, uniqueId, jointName); //TODO make sure that ID is unique


//    kdlJoint.name = uniqueId;

    LOG(INFO) << "Joint id: " << colladaJoint.getJointId();
    LOG(INFO) << "Joint name: " << colladaJoint.getJointName();


    return colladaJoint;
}
void ToasterSimuRobotReader::robotJointStateCallBack(const toaster_msgs::RobotListStamped::ConstPtr& msg) {
    //std::cout << "[area_manager][DEBUG] new data for robot received" << std::endl;

    Robot* curRobot;
    double roll, pitch, yaw;
    for (unsigned int i = 0; i < msg->robotList.size(); i++) {

        // If this robot is not assigned we have to allocate data.
        if (lastConfig_.find(msg->robotList[i].meAgent.meEntity.id) == lastConfig_.end()) {
            curRobot = new Robot(msg->robotList[i].meAgent.meEntity.id);
        } else
            curRobot = lastConfig_[msg->robotList[i].meAgent.meEntity.id];

        std::vector<double> robOrientation;
        double roll, pitch, ywa;
        bg::model::point<double, 3, bg::cs::cartesian> robPosition;

        Mobility curRobMobility = FULL;
        curRobot->setId(msg->robotList[i].meAgent.meEntity.id);
        curRobot->setName(msg->robotList[i].meAgent.meEntity.name);

        curRobot->setMobility(curRobMobility);
        curRobot->setTime(msg->robotList[i].meAgent.meEntity.time);
        curRobot->busyHands_ = msg->robotList[i].meAgent.busyHands;

        robPosition.set<0>(msg->robotList[i].meAgent.meEntity.pose.position.x);
        robPosition.set<1>(msg->robotList[i].meAgent.meEntity.pose.position.y);
        robPosition.set<2>(msg->robotList[i].meAgent.meEntity.pose.position.z);
        curRobot->setPosition(robPosition);


        tf::Quaternion q;

        tf::quaternionMsgToTF(msg->robotList[i].meAgent.meEntity.pose.orientation, q);
        tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

        robOrientation.push_back(roll);
        robOrientation.push_back(pitch);
        robOrientation.push_back(yaw);
        curRobot->setOrientation(robOrientation);

        lastConfig_[curRobot->getId()] = curRobot;

        //TODO: fullRobot case
        if (msg->robotList[i].meAgent.skeletonJoint.size() > 0) {
            Joint * curJnt;
            for (unsigned int i_jnt = 0; i_jnt < msg->robotList[i].meAgent.skeletonJoint.size(); i_jnt++) {

                // If this joint is not assigned we have to allocate data.
                if (lastConfig_[curRobot->getId()]->skeleton_[msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.name ] == NULL) {
                    curJnt = new Joint(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.id, msg->robotList[i].meAgent.meEntity.id);
                } else
                    curJnt = lastConfig_[curRobot->getId()]->skeleton_[msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.name ];

                std::vector<double> jointOrientation;
                bg::model::point<double, 3, bg::cs::cartesian> jointPosition;

                curJnt->setName(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.name);
                curJnt->setAgentId(curRobot->getId());
                curJnt->setTime(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.time);

                jointPosition.set<0>(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.position.x);
                jointPosition.set<1>(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.position.y);
                jointPosition.set<2>(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.position.z);
                curJnt->setPosition(jointPosition);


                tf::Quaternion q;

                tf::quaternionMsgToTF(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.orientation, q);
                tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

                jointOrientation.push_back(roll);
                jointOrientation.push_back(pitch);
                jointOrientation.push_back(yaw);
                curJnt->setOrientation(jointOrientation);

                curJnt->position = msg->robotList[i].meAgent.skeletonJoint[i_jnt].position;

                lastConfig_[curRobot->getId()]->skeleton_[curJnt->getName()] = curJnt;
            }
        }
    }
}
示例#17
0
void ToasterHumanReader::humanJointStateCallBack(const toaster_msgs::HumanList::ConstPtr& msg) {
    //std::cout << "[area_manager][DEBUG] new data for human received with time " << msg->humanList[0].meAgent.meEntity.time  << std::endl;
    Human * curHuman;
    for (unsigned int i = 0; i < msg->humanList.size(); i++) {

        // If this human is not assigned we have to allocate data.
        if (lastConfig_.find(msg->humanList[i].meAgent.meEntity.id) == lastConfig_.end()) {
            curHuman = new Human(msg->humanList[i].meAgent.meEntity.id);
        } else
            curHuman = lastConfig_[msg->humanList[i].meAgent.meEntity.id];

        std::vector<double> humanOrientation;
        bg::model::point<double, 3, bg::cs::cartesian> humanPosition;

        Mobility curHumanMobility = FULL;
        curHuman->setId(msg->humanList[i].meAgent.meEntity.id);
        curHuman->setName(msg->humanList[i].meAgent.meEntity.name);

        curHuman->setMobility(curHumanMobility);
        curHuman->setTime(msg->humanList[i].meAgent.meEntity.time);
        curHuman->busyHands_ = msg->humanList[i].meAgent.busyHands;

        humanPosition.set<0>(msg->humanList[i].meAgent.meEntity.positionX);
        humanPosition.set<1>(msg->humanList[i].meAgent.meEntity.positionY);
        humanPosition.set<2>(msg->humanList[i].meAgent.meEntity.positionZ);
        curHuman->setPosition(humanPosition);

        humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationRoll);
        humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationPitch);
        humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationYaw);
        curHuman->setOrientation(humanOrientation);

        lastConfig_[curHuman->getId()] = curHuman;

        //TODO: fullHuman
        if (fullHuman_) {
            Joint * curJnt;
            for (unsigned int i_jnt = 0; i_jnt < msg->humanList[i].meAgent.skeletonNames.size(); i_jnt++) {

                // If this joint is not assigned we have to allocate data.
                if (lastConfig_[curHuman->getId()]->skeleton_[msg->humanList[i].meAgent.skeletonNames[i_jnt] ] == NULL) {
                    curJnt = new Joint(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.id, msg->humanList[i].meAgent.meEntity.id);
                } else
                    curJnt = lastConfig_[curHuman->getId()]->skeleton_[msg->humanList[i].meAgent.skeletonNames[i_jnt] ];

                std::vector<double> jointOrientation;
                bg::model::point<double, 3, bg::cs::cartesian> jointPosition;

                curJnt->setName(msg->humanList[i].meAgent.skeletonNames[i_jnt]);
                curJnt->setAgentId(curHuman->getId());
                curJnt->setTime(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.time);

                jointPosition.set<0>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionX);
                jointPosition.set<1>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionY);
                jointPosition.set<2>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionZ);
                curJnt->setPosition(jointPosition);

                jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationRoll);
                jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationPitch);
                jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationYaw);
                curJnt->setOrientation(jointOrientation);

                lastConfig_[curHuman->getId()]->skeleton_[curJnt->getName()] = curJnt;
            }
        }
    }
}
/**
 * Convert an OpenSim Joint into the equivalent SIMM joint, only subset of joint types are supported
 * 
*/
void SimbodySimmModel::convertJoint(const Joint& joint)
{

   string parentName;
   string childName;
    bool parentJointAdded = addExtraJoints(joint, parentName, childName);

   SimbodySimmJoint* ssj = new SimbodySimmJoint(joint.getName(), parentName, childName);

    // If parentJointAdded is false, that means the position and orientation in the
    // parent can be merged with the primary joint. So begin making the joint by
    // adding the non-zero components to the SimbodySimmJoint.
    if (parentJointAdded == false) {
        int rotationsSoFar = 0;
        SimTK::Vec3 location(0);
        SimTK::Vec3 orientation(0);
        const PhysicalOffsetFrame* offset =
            dynamic_cast<const PhysicalOffsetFrame*>(&joint.getParentFrame());
        if (offset) {
            location = offset->get_translation();
            orientation = offset->get_orientation();
        }


        if (NOT_EQUAL_WITHIN_ERROR(location[0], 0.0))
            ssj->addConstantDof("tx", NULL, location[0]);
        if (NOT_EQUAL_WITHIN_ERROR(location[1], 0.0))
            ssj->addConstantDof("ty", NULL, location[1]);
        if (NOT_EQUAL_WITHIN_ERROR(location[2], 0.0))
            ssj->addConstantDof("tz", NULL, location[2]);
        if (NOT_EQUAL_WITHIN_ERROR(orientation[0], 0.0))
            ssj->addConstantDof(_rotationNames[rotationsSoFar++], defaultAxes[0], orientation[0] * 180.0 / SimTK::Pi);
        if (NOT_EQUAL_WITHIN_ERROR(orientation[1], 0.0))
            ssj->addConstantDof(_rotationNames[rotationsSoFar++], defaultAxes[1], orientation[1] * 180.0 / SimTK::Pi);
        if (NOT_EQUAL_WITHIN_ERROR(orientation[2], 0.0))
            ssj->addConstantDof(_rotationNames[rotationsSoFar++], defaultAxes[2], orientation[2] * 180.0 / SimTK::Pi);
    }

    if (joint.getConcreteClassName()==("WeldJoint")) {
        // Nothing to do.
    } else if (joint.getConcreteClassName()==("PinJoint")) {
        int index = 0;
        string coordName = joint.get_coordinates(index).getName();
        SimTK::Vec3 axis(0.0, 0.0, 1.0); // Pin joints always rotate about the Z axis.
        ssj->addFunctionDof(axis, coordName, 0, Coordinate::Rotational);
    } else if (joint.getConcreteClassName()==("SliderJoint")) {
        int index = 0;
        string coordName = joint.get_coordinates(index).getName();
        SimTK::Vec3 axis(1.0, 0.0, 0.0); // Slider joints always translate along the X axis.
        ssj->addFunctionDof(axis, coordName, 0, Coordinate::Translational);
    } else if (joint.getConcreteClassName()==("EllipsoidJoint")) {
        // NOTE: Ellipsoid joints cannot be converted into SIMM joints.
    } else if (joint.getConcreteClassName()==("FreeJoint")) {
        SimTK::Vec3 xaxis(1.0, 0.0, 0.0);
        SimTK::Vec3 yaxis(0.0, 1.0, 0.0);
        SimTK::Vec3 zaxis(0.0, 0.0, 1.0);
        int index = 0;
        ssj->addFunctionDof(xaxis, joint.get_coordinates(index++).getName(), 0, Coordinate::Translational);
        ssj->addFunctionDof(yaxis, joint.get_coordinates(index++).getName(), 0, Coordinate::Translational);
        ssj->addFunctionDof(zaxis, joint.get_coordinates(index++).getName(), 0, Coordinate::Translational);
        ssj->addFunctionDof(xaxis, joint.get_coordinates(index++).getName(), 0, Coordinate::Rotational);
        ssj->addFunctionDof(yaxis, joint.get_coordinates(index++).getName(), 0, Coordinate::Rotational);
        ssj->addFunctionDof(zaxis, joint.get_coordinates(index).getName(), 0, Coordinate::Rotational);
    } else if (joint.getConcreteClassName()==("CustomJoint")) {
        const CustomJoint* cj = (CustomJoint*)(&joint);

        // Add the joint's transform axes to the SimbodySimmJoint.
        const SpatialTransform& dofs = cj->getSpatialTransform();
        // Custom joints have the rotational DOFs specified first, but the translations are applied first
        // so you want to process them first here.
        static int order[] = {3, 4, 5, 0, 1, 2};
        for (int i=0; i<6; i++) {
            const TransformAxis* ta = &dofs[order[i]];
            if (ta->getCoordinateNames().size() > 0) { // transform axis is unused if it has no coordinate names
                const Coordinate* coord = NULL;
                const Coordinate* independentCoord = NULL;
                const Function* constraintFunc = NULL;
                int ix = -1;
                Coordinate::MotionType motionType = (order[i]<3) ? Coordinate::Rotational : Coordinate::Translational;
                if (ta->getCoordinateNames().size() > 0)
                    ix = cj->getProperty_coordinates().findIndexForName(ta->getCoordinateNames()[0]);
                if (ix >= 0) {
                    coord = &cj->get_coordinates(ix);
                    constraintFunc = isDependent(coord, &independentCoord);
                }
                if (constraintFunc != NULL) {  // dof is constrained to a coordinate in another joint
                    ssj->addFunctionDof(ta->getAxis(), independentCoord->getName(), addJointFunction(constraintFunc, independentCoord->getMotionType(), motionType), motionType);
                } else {
                    if (ta->hasFunction())
                        constraintFunc = &ta->getFunction();
                    if (constraintFunc) // dof is constrained to a coordinate in this joint
                        ssj->addFunctionDof(ta->getAxis(), coord->getName(), addJointFunction(constraintFunc, coord->getMotionType(), motionType), motionType);
                    else // dof is unconstrained
                        ssj->addFunctionDof(ta->getAxis(), coord->getName(), 0, motionType);
                }
            }
        }
    }

    ssj->finalize();
    addSimmJoint(ssj);
}
示例#19
0
//==============================================================================
TEST_F(JOINTS, CONVENIENCE_FUNCTIONS)
{
  // -- set up the root BodyNode
  BodyNode* root = new BodyNode("root");
  WeldJoint* rootjoint = new WeldJoint("base");
  root->setParentJoint(rootjoint);

  // -- set up the FreeJoint
  BodyNode* freejoint_bn = new BodyNode("freejoint_bn");
  FreeJoint* freejoint = new FreeJoint("freejoint");

  freejoint_bn->setParentJoint(freejoint);
  root->addChildBodyNode(freejoint_bn);

  freejoint->setTransformFromParentBodyNode(random_transform());
  freejoint->setTransformFromChildBodyNode(random_transform());

  // -- set up the EulerJoint
  BodyNode* eulerjoint_bn = new BodyNode("eulerjoint_bn");
  EulerJoint* eulerjoint = new EulerJoint("eulerjoint");

  eulerjoint_bn->setParentJoint(eulerjoint);
  root->addChildBodyNode(eulerjoint_bn);

  eulerjoint->setTransformFromParentBodyNode(random_transform());
  eulerjoint->setTransformFromChildBodyNode(random_transform());

  // -- set up the BallJoint
  BodyNode* balljoint_bn = new BodyNode("balljoint_bn");
  BallJoint* balljoint = new BallJoint("balljoint");

  balljoint_bn->setParentJoint(balljoint);
  root->addChildBodyNode(balljoint_bn);

  balljoint->setTransformFromParentBodyNode(random_transform());
  balljoint->setTransformFromChildBodyNode(random_transform());

  // -- set up Skeleton and compute forward kinematics
  Skeleton* skel = new Skeleton;
  skel->addBodyNode(root);
  skel->addBodyNode(freejoint_bn);
  skel->addBodyNode(eulerjoint_bn);
  skel->addBodyNode(balljoint_bn);
  skel->init();

  // Test a hundred times
  for(size_t n=0; n<100; ++n)
  {
    // -- convert transforms to positions and then positions back to transforms
    Eigen::Isometry3d desired_freejoint_tf = random_transform();
    freejoint->setPositions(FreeJoint::convertToPositions(desired_freejoint_tf));
    Eigen::Isometry3d actual_freejoint_tf = FreeJoint::convertToTransform(
          freejoint->getPositions());

    Eigen::Isometry3d desired_eulerjoint_tf = random_transform();
    desired_eulerjoint_tf.translation() = Eigen::Vector3d::Zero();
    eulerjoint->setPositions(
          eulerjoint->convertToPositions(desired_eulerjoint_tf.linear()));
    Eigen::Isometry3d actual_eulerjoint_tf = eulerjoint->convertToTransform(
          eulerjoint->getPositions());

    Eigen::Isometry3d desired_balljoint_tf = random_transform();
    desired_balljoint_tf.translation() = Eigen::Vector3d::Zero();
    balljoint->setPositions(
          BallJoint::convertToPositions(desired_balljoint_tf.linear()));
    Eigen::Isometry3d actual_balljoint_tf = BallJoint::convertToTransform(
          balljoint->getPositions());

    skel->computeForwardKinematics(true, false, false);

    // -- collect everything so we can cycle through the tests
    std::vector<Joint*> joints;
    std::vector<BodyNode*> bns;
    std::vector<Eigen::Isometry3d> desired_tfs;
    std::vector<Eigen::Isometry3d> actual_tfs;

    joints.push_back(freejoint);
    bns.push_back(freejoint_bn);
    desired_tfs.push_back(desired_freejoint_tf);
    actual_tfs.push_back(actual_freejoint_tf);

    joints.push_back(eulerjoint);
    bns.push_back(eulerjoint_bn);
    desired_tfs.push_back(desired_eulerjoint_tf);
    actual_tfs.push_back(actual_eulerjoint_tf);

    joints.push_back(balljoint);
    bns.push_back(balljoint_bn);
    desired_tfs.push_back(desired_balljoint_tf);
    actual_tfs.push_back(actual_balljoint_tf);

    for(size_t i=0; i<joints.size(); ++i)
    {
      Joint* joint = joints[i];
      BodyNode* bn = bns[i];
      Eigen::Isometry3d tf = desired_tfs[i];

      bool check_transform_conversion =
          equals(predict_joint_transform(joint, tf).matrix(),
                 get_relative_transform(bn, bn->getParentBodyNode()).matrix());
      EXPECT_TRUE(check_transform_conversion);

      if(!check_transform_conversion)
      {
        std::cout << "[" << joint->getName() << " Failed]\n";
        std::cout << "Predicted:\n" << predict_joint_transform(joint, tf).matrix()
                  << "\n\nActual:\n"
                  << get_relative_transform(bn, bn->getParentBodyNode()).matrix()
                  << "\n\n";
      }

      bool check_full_cycle = equals(desired_tfs[i].matrix(),
                                     actual_tfs[i].matrix());
      EXPECT_TRUE(check_full_cycle);

      if(!check_full_cycle)
      {
        std::cout << "[" << joint->getName() << " Failed]\n";
        std::cout << "Desired:\n" << desired_tfs[i].matrix()
                  << "\n\nActual:\n" << actual_tfs[i].matrix()
                  << "\n\n";
      }
    }
  }
}
示例#20
0
TEST(Skeleton, Restructuring)
{
  std::vector<SkeletonPtr> skeletons = getSkeletons();

#ifndef NDEBUG
  size_t numIterations = 10;
#else
  size_t numIterations = 2*skeletons.size();
#endif

  // Test moves within the current Skeleton
  for(size_t i=0; i<numIterations; ++i)
  {
    size_t index = floor(math::random(0, skeletons.size()));
    index = std::min(index, skeletons.size()-1);
    SkeletonPtr skeleton = skeletons[index];
    SkeletonPtr original = skeleton->clone();

    size_t maxNode = skeleton->getNumBodyNodes()-1;
    BodyNode* bn1 = skeleton->getBodyNode(floor(math::random(0, maxNode)));
    BodyNode* bn2 = skeleton->getBodyNode(ceil(math::random(0, maxNode)));

    if(bn1 == bn2)
    {
      --i;
      continue;
    }

    BodyNode* child = bn1->descendsFrom(bn2)? bn1 : bn2;
    BodyNode* parent = child == bn1? bn2 : bn1;

    child->moveTo(parent);

    EXPECT_TRUE(skeleton->getNumBodyNodes() == original->getNumBodyNodes());
    if(skeleton->getNumBodyNodes() == original->getNumBodyNodes())
    {
      for(size_t j=0; j<skeleton->getNumBodyNodes(); ++j)
      {
        // Make sure no BodyNodes have been lost or gained in translation
        std::string name = original->getBodyNode(j)->getName();
        BodyNode* bn = skeleton->getBodyNode(name);
        EXPECT_FALSE(bn == nullptr);
        if(bn)
          EXPECT_TRUE(bn->getName() == name);

        name = skeleton->getBodyNode(j)->getName();
        bn = original->getBodyNode(name);
        EXPECT_FALSE(bn == nullptr);
        if(bn)
          EXPECT_TRUE(bn->getName() == name);


        // Make sure no Joints have been lost or gained in translation
        name = original->getJoint(j)->getName();
        Joint* joint = skeleton->getJoint(name);
        EXPECT_FALSE(joint == nullptr);
        if(joint)
          EXPECT_TRUE(joint->getName() == name);

        name = skeleton->getJoint(j)->getName();
        joint = original->getJoint(name);
        EXPECT_FALSE(joint == nullptr);
        if(joint)
          EXPECT_TRUE(joint->getName() == name);
      }
    }

    EXPECT_TRUE(skeleton->getNumDofs() == original->getNumDofs());
    for(size_t j=0; j<skeleton->getNumDofs(); ++j)
    {
      std::string name = original->getDof(j)->getName();
      DegreeOfFreedom* dof = skeleton->getDof(name);
      EXPECT_FALSE(dof == nullptr);
      if(dof)
        EXPECT_TRUE(dof->getName() == name);

      name = skeleton->getDof(j)->getName();
      dof = original->getDof(name);
      EXPECT_FALSE(dof == nullptr);
      if(dof)
        EXPECT_TRUE(dof->getName() == name);
    }
  }

  // Test moves between Skeletons
  for(size_t i=0; i<numIterations; ++i)
  {
    size_t fromIndex = floor(math::random(0, skeletons.size()));
    fromIndex = std::min(fromIndex, skeletons.size()-1);
    SkeletonPtr fromSkel = skeletons[fromIndex];

    if(fromSkel->getNumBodyNodes() == 0)
    {
      --i;
      continue;
    }

    size_t toIndex = floor(math::random(0, skeletons.size()));
    toIndex = std::min(toIndex, skeletons.size()-1);
    SkeletonPtr toSkel = skeletons[toIndex];

    if(toSkel->getNumBodyNodes() == 0)
    {
      --i;
      continue;
    }

    BodyNode* childBn = fromSkel->getBodyNode(
          floor(math::random(0, fromSkel->getNumBodyNodes()-1)));
    BodyNode* parentBn = toSkel->getBodyNode(
          floor(math::random(0, toSkel->getNumBodyNodes()-1)));

    if(fromSkel == toSkel)
    {
      if(childBn == parentBn)
      {
        --i;
        continue;
      }

      if(parentBn->descendsFrom(childBn))
      {
        BodyNode* tempBn = childBn;
        childBn = parentBn;
        parentBn = tempBn;

        SkeletonPtr tempSkel = fromSkel;
        fromSkel = toSkel;
        toSkel = tempSkel;
      }
    }

    BodyNode* originalParent = childBn->getParentBodyNode();
    std::vector<BodyNode*> subtree;
    constructSubtree(subtree, childBn);

    // Move to a new Skeleton
    childBn->moveTo(parentBn);

    // Make sure all the objects have moved
    for(size_t j=0; j<subtree.size(); ++j)
    {
      BodyNode* bn = subtree[j];
      EXPECT_TRUE(bn->getSkeleton() == toSkel);
    }

    // Move to the Skeleton's root while producing a new Joint type
    sub_ptr<Joint> originalJoint = childBn->getParentJoint();
    childBn->moveTo<FreeJoint>(nullptr);

    // The original parent joint should be deleted now
    EXPECT_TRUE(originalJoint == nullptr);

    // The BodyNode should now be a root node
    EXPECT_TRUE(childBn->getParentBodyNode() == nullptr);

    // The subtree should still be in the same Skeleton
    for(size_t j=0; j<subtree.size(); ++j)
    {
      BodyNode* bn = subtree[j];
      EXPECT_TRUE(bn->getSkeleton() == toSkel);
    }

    // Create some new Skeletons and mangle them all up

    childBn->copyTo<RevoluteJoint>(fromSkel, originalParent);

    SkeletonPtr temporary = childBn->split("temporary");
    SkeletonPtr other_temporary =
        childBn->split<PrismaticJoint>("other temporary");
    SkeletonPtr another_temporary = childBn->copyAs("another temporary");
    SkeletonPtr last_temporary = childBn->copyAs<ScrewJoint>("last temporary");

    childBn->copyTo(another_temporary->getBodyNode(
                      another_temporary->getNumBodyNodes()-1));
    childBn->copyTo<PlanarJoint>(another_temporary->getBodyNode(0));
    childBn->copyTo<TranslationalJoint>(temporary, nullptr);
    childBn->moveTo(last_temporary,
        last_temporary->getBodyNode(last_temporary->getNumBodyNodes()-1));
    childBn->moveTo<BallJoint>(last_temporary, nullptr);
    childBn->moveTo<EulerJoint>(last_temporary,
                                last_temporary->getBodyNode(0));
    childBn->changeParentJointType<FreeJoint>();

    // Test the non-recursive copying
    if(toSkel->getNumBodyNodes() > 1)
    {
      SkeletonPtr singleBodyNode =
          toSkel->getBodyNode(0)->copyAs("single", false);
      EXPECT_TRUE(singleBodyNode->getNumBodyNodes() == 1);

      std::pair<Joint*, BodyNode*> singlePair =
          toSkel->getBodyNode(0)->copyTo(nullptr, false);
      EXPECT_TRUE(singlePair.second->getNumChildBodyNodes() == 0);
    }

    // Check that the mangled Skeletons are all self-consistent
    check_self_consistency(fromSkel);
    check_self_consistency(toSkel);
    check_self_consistency(temporary);
    check_self_consistency(other_temporary);
    check_self_consistency(another_temporary);
    check_self_consistency(last_temporary);
  }
}