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;
}
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;
}
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));
}
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;
}
//==============================================================================
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();
}
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;
}
/* 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);
}
}
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);
}
//---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);
}
}
/**
* 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());
}
}
//==============================================================================
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;
}
}
}
}
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);
}
//==============================================================================
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";
}
}
}
}
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);
}
}