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 ;
}
