int JointInvertPolarity(const KDL::Joint & old_joint, const KDL::Frame & old_f_tip, KDL::Joint & new_joint, KDL::Frame & new_f_tip)
{
switch(old_joint.getType())
{
case Joint::RotAxis:
case Joint::RotX:
case Joint::RotY:
case Joint::RotZ:
//\todo document this
new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_joint.JointOrigin()));
new_joint = Joint(old_joint.getName(),-(old_f_tip.M.Inverse()*old_f_tip.p), -(old_f_tip.M.Inverse()*old_joint.JointAxis()), Joint::RotAxis);
break;
case Joint::TransAxis:
case Joint::TransX:
case Joint::TransY:
case Joint::TransZ:
new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_f_tip.p));
new_joint = Joint(old_joint.getName(),-(old_f_tip.M.Inverse()*old_joint.JointOrigin()), -(old_f_tip.M.Inverse()*old_joint.JointAxis()), Joint::TransAxis);
break;
case Joint::None:
default:
new_f_tip = KDL::Frame(old_f_tip.M.Inverse(),-(old_f_tip.M.Inverse()*old_f_tip.p));
new_joint = Joint(old_joint.getName(), Joint::None);
break;
}
#ifndef NDEBUG
std::cerr << "Exiting joint polarity inversion, checking all is working" << std::endl;
double q = 0.83;
std::cerr << old_joint.pose(q)*old_f_tip*new_joint.pose(q)*new_f_tip << std::endl;
#endif
return 0;
}
bool perform_ik(trac_ik_baxter::GetConstrainedPositionIK::Request &request,
trac_ik_baxter::GetConstrainedPositionIK::Response &response) {
if(!this->_ready)
return false;
double initial_tolerance = 1e-5;
if(request.num_steps == 0)
request.num_steps = 1;
if(request.end_tolerance < initial_tolerance) {
request.num_steps = 1;
ROS_WARN("Invalid IK end tolerance, using the default");
}
double step = (request.end_tolerance - initial_tolerance)/request.num_steps;
int rc;
KDL::JntArray result;
sensor_msgs::JointState joint_state;
for(uint segment=0; segment<this->_chain.getNrOfSegments(); ++segment) {
KDL::Joint joint = this->_chain.getSegment(segment).getJoint();
if(joint.getType()!=KDL::Joint::None)
joint_state.name.push_back(joint.getName());
}
bool seeds_provided = request.seed_angles.size() == request.pose_stamp.size();
for(uint point=0; point<request.pose_stamp.size(); ++point) {
KDL::Frame end_effector_pose(KDL::Rotation::Quaternion(request.pose_stamp[point].pose.orientation.x,
request.pose_stamp[point].pose.orientation.y,
request.pose_stamp[point].pose.orientation.z,
request.pose_stamp[point].pose.orientation.w),
KDL::Vector(request.pose_stamp[point].pose.position.x,
request.pose_stamp[point].pose.position.y,
request.pose_stamp[point].pose.position.z));
KDL::JntArray seed(this->_chain.getNrOfJoints());
if(seeds_provided)
seed = JointState2JntArray(request.seed_angles[point]);
for(uint num_attempts=0; num_attempts<request.num_steps; ++num_attempts) {
std::stringstream ss;
double tolerance = initial_tolerance + num_attempts*step;
ss << "Attempt num " << num_attempts +1 << " with tolerence " << tolerance << std::endl;
ROS_INFO("%s", ss.str().c_str());
this->_tracik_solver->setEpsilon(tolerance);
rc = this->_tracik_solver->CartToJnt(seeds_provided? seed: *(this->_nominal),
end_effector_pose, result);
if(rc>=0) break;
}
for(uint joint=0; joint<this->_chain.getNrOfJoints(); ++joint) {
joint_state.position.push_back(result(joint));
}
response.joints.push_back(joint_state);
response.isValid.push_back(rc>=0);
}
return true;
}
sensor_msgs::JointState init_message(const KDL::Chain &chain) {
sensor_msgs::JointState msg;
for (unsigned int i=0; i < chain.getNrOfSegments(); ++i) {
KDL::Segment segment = chain.getSegment(i);
KDL::Joint joint = segment.getJoint();
if (joint.getType() == KDL::Joint::JointType::None) continue;
msg.name.push_back(joint.getName());
msg.position.push_back(0);
}
return msg;
}
KDL::Frame getH_new_old(KDL::Joint jnt, KDL::Frame frameToTip)
{
KDL::Frame H_new_old;
if( (jnt.JointOrigin()-frameToTip.p).Norm() < 1e-12 || jnt.getType() == KDL::Joint::None ) {
//No need of changing link frame
H_new_old = KDL::Frame::Identity();
} else {
std::cerr << "[WARN] the reference frame of link connected to joint " << jnt.getName() << " has to be shifted to comply to URDF constraints" << std::endl;
//H_new_old = (KDL::Frame(jnt.JointOrigin())*KDL::Frame(frameToTip.M)).Inverse()*frameToTip;
H_new_old = KDL::Frame(frameToTip.M.Inverse()*(frameToTip.p-jnt.JointOrigin()));
}
return H_new_old;
}
/**
*
* @param jnt the KDL::Joint to convert (axis and origin expressed in the
* predecessor frame of reference, as by KDL convention)
* @param frameToTip the predecessor/successor frame transformation
* @param H_new_old_predecessor in the case the predecessor frame is being
* modified to comply to URDF constraints (frame origin on the joint axis)
* this matrix the transformation from the old frame to the new frame (H_new_old)
* @param H_new_old_successor in the case the successor frame is being
* modified to comply to URDF constraints (frame origin on the joint axis)
* this matrix the transformation from the old frame to the new frame (H_new_old)
*/
urdf::Joint toUrdf(const KDL::Joint & jnt,
const KDL::Frame & frameToTip,
const KDL::Frame & H_new_old_predecessor,
KDL::Frame & H_new_old_successor)
{
//URDF constaints the successor link frame origin to lay on the axis
//of the joint ( see : http://www.ros.org/wiki/urdf/XML/joint )
//Then if the JointOrigin of the KDL joint is not zero, it is necessary
//to move the link frame (then it is necessary to change also the spatial inertia)
//and the definition of the childrens of the successor frame
urdf::Joint ret;
ret.name = jnt.getName();
H_new_old_successor = getH_new_old(jnt,frameToTip);
ret.parent_to_joint_origin_transform = toUrdf(H_new_old_predecessor*frameToTip*(H_new_old_successor.Inverse()));
switch(jnt.getType())
{
case KDL::Joint::RotAxis:
case KDL::Joint::RotX:
case KDL::Joint::RotY:
case KDL::Joint::RotZ:
//using continuos if no joint limits are specified
ret.type = urdf::Joint::CONTINUOUS;
//in urdf, the joint axis is expressed in the joint/successor frame
//in kdl, the joint axis is expressed in the predecessor rame
ret.axis = toUrdf(((frameToTip).M.Inverse((jnt.JointAxis()))));
break;
case KDL::Joint::TransAxis:
case KDL::Joint::TransX:
case KDL::Joint::TransY:
case KDL::Joint::TransZ:
ret.type = urdf::Joint::PRISMATIC;
//in urdf, the joint axis is expressed in the joint/successor frame
//in kdl, the joint axis is expressed in the predecessor rame
ret.axis = toUrdf((frameToTip.M.Inverse(jnt.JointAxis())));
break;
default:
std::cerr << "[WARN] Converting unknown joint type of joint " << jnt.getTypeName() << " into a fixed joint" << std::endl;
case KDL::Joint::None:
ret.type = urdf::Joint::FIXED;
}
return ret;
}
static inline YAML::Node extract(const std::string& start_link, const std::string& end_link, const KDL::Chain& chain)
{
std::string var_suffix = "_var";
std::string frame_suffix = "_frame";
std::string expression_name = "fk";
std::vector<YAML::Node> input_vars;
std::vector<YAML::Node> joint_frames;
YAML::Node frame_mul;
YAML::Node axis_angle_null;
axis_angle_null["axis-angle"].push_back(get_vector3(1, 0, 0));
axis_angle_null["axis-angle"].push_back(0);
int input_var_index = 0;
for (std::vector<KDL::Segment>::const_iterator it = chain.segments.begin(); it != chain.segments.end(); ++it)
{
KDL::Joint joint = it->getJoint();
std::string var_name = joint.getName() + var_suffix;
std::string frame_name = joint.getName() + frame_suffix;
YAML::Node joint_frame;
if (joint.getType() != KDL::Joint::None)
{
// Set input variable definition
YAML::Node input_var;
input_var[joint.getName() + var_suffix]["input-var"] = input_var_index;
input_var_index++;
input_vars.push_back(input_var);
// Set joint transform
YAML::Node translation;
YAML::Node rotation;
// Set joint axis
YAML::Node joint_axis = get_vector3(joint.JointAxis());
if (joint.getType() == KDL::Joint::TransAxis)
{
YAML::Node scale_vec;
scale_vec["scale-vector"].push_back(var_name);
scale_vec["scale-vector"].push_back(joint_axis);
translation["vector-add"].push_back(scale_vec);
translation["vector-add"].push_back(get_vector3(joint.JointOrigin()));
rotation = axis_angle_null;
}
else if (joint.getType() == KDL::Joint::RotAxis)
{
translation = get_vector3(joint.JointOrigin());
rotation["axis-angle"].push_back(joint_axis);
rotation["axis-angle"].push_back(var_name);
}
// Create frame and add to list
YAML::Node joint_transform;
joint_transform["frame"].push_back(rotation);
joint_transform["frame"].push_back(translation);
joint_frame[frame_name]["frame-mul"].push_back(joint_transform);
}
std::vector<YAML::Node> f_tip_nodes = get_frame_tip_nodes(*it);
for (std::vector<YAML::Node>::iterator f_tip_it = f_tip_nodes.begin(); f_tip_it != f_tip_nodes.end(); ++f_tip_it)
{
joint_frame[frame_name]["frame-mul"].push_back(*f_tip_it);
}
if (joint_frame.size() > 0)
{
joint_frames.push_back(joint_frame);
frame_mul.push_back(frame_name);
}
}
// Merge nodes
YAML::Node node;
for (std::vector<YAML::Node>::iterator it = input_vars.begin(); it != input_vars.end(); ++it)
{
node.push_back(*it);
}
for (std::vector<YAML::Node>::iterator it = joint_frames.begin(); it != joint_frames.end(); ++it)
{
node.push_back(*it);
}
YAML::Node fk_def;
fk_def[expression_name]["frame-mul"] = frame_mul;
node.push_back(fk_def);
return node;
}
bool TransformationCalculator::is_valid_joint_type(const KDL::Joint& joint){
if(joint.getType() == KDL::Joint::None)
return false;
return true;
}