bool KDLColladaLibraryJointsExporter::doExport(vector <Chain>& kdlChains) { if (kdlChains.empty()) { LOG(DEBUG) << "KDL Chains array is empty, has nothing to export"; return false; } LOG(WARNING) << "Taking first element from the KDL chain array"; Chain kdlChain = kdlChains[0]; openLibrary(); unsigned int jointNr = kdlChain.getNrOfSegments(); for (unsigned int i = 0; i < jointNr; i++) { Segment segment = kdlChain.getSegment(i); Joint kdlJoint = segment.getJoint(); string uniqueId = defaultJointIdPrefix + toString(i); COLLADASW::Joint colladaJoint = makeColladaSWJoint(COLLADASW::LibraryJoints::mSW, kdlJoint, uniqueId); LOG(INFO) << "Joint id: " << colladaJoint.getJointId(); LOG(INFO) << "Joint name: " << colladaJoint.getJointName(); addJoint(colladaJoint); joints.push_back(colladaJoint); } closeLibrary(); return true; }
bool Tree::addChain(const Chain& chain, const std::string& hook_name) { string parent_name = hook_name; for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) { if (this->addSegment(chain.getSegment(i), parent_name)) parent_name = chain.getSegment(i).getName(); else return false; } return true; }
bool Tree::addChain(const Chain& chain, const std::string& chain_name, const std::string& hook_name) { string parent_name = hook_name; for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) { ostringstream segment_name; segment_name << chain_name << "Segment" << i; if (this->addSegment(chain.getSegment(i), segment_name.str(), parent_name)) parent_name = segment_name.str(); else return false; } return true; }
/* * To compute the jacobian, we start at the base, and iterate up the chain. We assume that the link we are currently on * is the last link in the chain, so the tip of our current link is effectively the end-effector of our chain. Once we compute * the jacobian for this pseudo-end effector, we move to the next link, and transform the previously computed jacobian to be for * the new end-effector. */ int LinkParamJacobianSolver::JointsToCartesian(const Chain& chain, const JntArray& joint_states, LinkParamJacobian& jac) { const unsigned int N = chain.getNrOfJoints() ; if ( joint_states.rows() != N ) // Sanity check on input data return -1 ; jac.links_.resize(N) ; Frame T_start_to_eef ; // Defines transform from the start of the chain to end-effector [of the part of the chain we've seen so far] Frame T_start_to_prev_eef = Frame::Identity() ; unsigned int joint_num = 0 ; // Keep track of which joint we're currently on. Since some joints are fixed, // the joint # could be different than the segment # for (unsigned int i=0; i<chain.getNrOfSegments(); i++) // Walk up the chain { const bool movable = chain.getSegment(i).getJoint().getType() != Joint::None ; // Not sure what the right answer is for dealing with fixed joints const bool process_segment = movable ; // For now, compute Jacobians only for movable joints. if(process_segment) T_start_to_eef = T_start_to_prev_eef * chain.getSegment(i).pose(joint_states(joint_num)) ; else T_start_to_eef = T_start_to_prev_eef * chain.getSegment(i).pose(0.0) ; Vector cur_segment_length = T_start_to_eef.p - T_start_to_prev_eef.p ; // Defines vector-length of the current segment in the global frame jac.changeRefPoint(cur_segment_length) ; // Walk the previously computed jacobians up the chain if (process_segment) { // Build Twists for the translation elements x,y,z. These translations occur in the base frame of the current segment Twist trans_twist[3] ; trans_twist[0].vel[0] = 1.0 ; trans_twist[0].vel[1] = 0.0 ; trans_twist[0].vel[2] = 0.0 ; trans_twist[1].vel[0] = 0.0 ; trans_twist[1].vel[1] = 1.0 ; trans_twist[1].vel[2] = 0.0 ; trans_twist[2].vel[0] = 0.0 ; trans_twist[2].vel[1] = 0.0 ; trans_twist[2].vel[2] = 1.0 ; // Build rotation from start to base frame of current link. This also includes the rotation from the current link's joint. Rotation R_start_to_cur_base = T_start_to_prev_eef.M * chain.getSegment(i).getJoint().pose(joint_states(joint_num)).M ; for (int j=0; j<3; j++) { trans_twist[j] = R_start_to_cur_base*trans_twist[j] ; // Rotate translations from the base frame of the current link to the start frame jac.links_[joint_num].trans_[j] = trans_twist[j] ; // Copy data into the output datatype } // Build Twists for incremental rotations at the end of the segment. These translations occur after the segment transform. Twist rot_twist[3] ; // First build twists in the [current] end-effector frame rot_twist[0].rot[0] = 1.0 ; rot_twist[0].rot[1] = 0.0 ; rot_twist[0].rot[2] = 0.0 ; rot_twist[1].rot[0] = 0.0 ; rot_twist[1].rot[1] = 1.0 ; rot_twist[1].rot[2] = 0.0 ; rot_twist[2].rot[0] = 0.0 ; rot_twist[2].rot[1] = 0.0 ; rot_twist[2].rot[2] = 1.0 ; // Now we need to transform these rotation twists to the base (beginning-of-segment) frame for (int j=0; j<3; j++) { rot_twist[j] = T_start_to_eef.M*rot_twist[j] ; jac.links_[joint_num].rot_[j] = rot_twist[j] ; // Copy data into the output datatype } } if (movable) joint_num++ ; T_start_to_prev_eef = T_start_to_eef ; } return 0 ; }