void ChainKinematics::getXInv(double (&t)[3], double (&rot)[9], const double* q, const int segmentNr) const {
int i,j;
KDL::JntArray qJA(Njnt_);
for(i=0; i<Njnt_; i++)
qJA(i) = q[i];
KDL::Frame frame;
//compute forward kinematics
checkSegmentNr(segmentNr);
if(fksolverPos_->JntToCart(qJA, frame, segmentNr) < 0) {
std::cerr << "ERROR: [ChainKinematics][getXInv] something went wrong during JntToCart! Exiting!" << std::endl;
exit(-1);
}
//get inverse transformation
frame = frame.Inverse();
for(i=0; i<3; i++) {
t[i] = frame.p(i);
for(j=0; j<3; j++)
rot[i*3+j] = frame.M(i,j);
}
}
void MultiTaskPriorityInverseKinematics::set_marker(KDL::Frame x, int index, int id)
{
sstr_.str("");
sstr_.clear();
if (links_index_[index] == -1)
sstr_<<"end_effector";
else
sstr_<<"link_"<<(links_index_[index]-1);
msg_marker_.markers[index].header.frame_id = "world";
msg_marker_.markers[index].header.stamp = ros::Time();
msg_marker_.markers[index].ns = sstr_.str();
msg_marker_.markers[index].id = id;
msg_marker_.markers[index].type = visualization_msgs::Marker::SPHERE;
msg_marker_.markers[index].action = visualization_msgs::Marker::ADD;
msg_marker_.markers[index].pose.position.x = x.p(0);
msg_marker_.markers[index].pose.position.y = x.p(1);
msg_marker_.markers[index].pose.position.z = x.p(2);
msg_marker_.markers[index].pose.orientation.x = 0.0;
msg_marker_.markers[index].pose.orientation.y = 0.0;
msg_marker_.markers[index].pose.orientation.z = 0.0;
msg_marker_.markers[index].pose.orientation.w = 1.0;
msg_marker_.markers[index].scale.x = 0.01;
msg_marker_.markers[index].scale.y = 0.01;
msg_marker_.markers[index].scale.z = 0.01;
msg_marker_.markers[index].color.a = 1.0;
msg_marker_.markers[index].color.r = 0.0;
msg_marker_.markers[index].color.g = 1.0;
msg_marker_.markers[index].color.b = 0.0;
}
void ChainKinematics::getX(double (&t)[3], double (&rot)[9], const double* q, const int segmentNr) const {
int i,j;
////DEBUG
//std::cout << "++++++++++++++++++++ DEBUG 0" << std::endl;
//std::cout << "q = ";
//for(i=0; i<Njnt_; i++)
//std::cout << q[i] << " " << std::endl;
//std::cout << std::endl;
KDL::JntArray qJA(Njnt_);
for(i=0; i<Njnt_; i++)
qJA(i) = q[i];
KDL::Frame frame;
//compute forward kinematics
checkSegmentNr(segmentNr);
if(fksolverPos_->JntToCart(qJA, frame, segmentNr) < 0) {
std::cerr << "ERROR: [ChainKinematics][getX] something went wrong during JntToCart! Exiting!" << std::endl;
exit(-1);
}
for(i=0; i<3; i++) {
t[i] = frame.p(i);
for(j=0; j<3; j++)
rot[i*3+j] = frame.M(i,j);
}
}
bool checkIdentity(KDL::Frame frame, double tol = 1e-4)
{
for(int i=0; i < 3; i++ )
{
for(int j=0; j < 3; j++ )
{
double err;
if( i == j )
{
err = fabs(frame.M(i,j)-1);
}
else
{
err = fabs(frame.M(i,j));
}
if( err < tol ) return false;
}
}
for(int i =0; i < 3; i++ )
{
double err = fabs(frame.p[i]);
if( err < tol ) return false;
}
return true;
}
Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p)
{
Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
for(int i=0; i < 3; i++)
{
for(int j=0; j<3; j++)
{
b(i,j) = p.M(i,j);
}
b(i,3) = p.p(i);
}
return b;
}
void leatherman::transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k)
{
k.p[0] = e(0,3);
k.p[1] = e(1,3);
k.p[2] = e(2,3);
k.M(0,0) = e(0,0);
k.M(0,1) = e(0,1);
k.M(0,2) = e(0,2);
k.M(1,0) = e(1,0);
k.M(1,1) = e(1,1);
k.M(1,2) = e(1,2);
k.M(2,0) = e(2,0);
k.M(2,1) = e(2,1);
k.M(2,2) = e(2,2);
}
//------------------------------------------------------------------------------
// Constructor hard-coded for LWR4 DH
//------------------------------------------------------------------------------
LWR4Kinematics::LWR4Kinematics(KDL::Frame _tool_to_ee){
tr_tool_to_ee = _tool_to_ee;
// just to prevent inverting at the run time
tr_ee_to_tool = _tool_to_ee.Inverse();
jlim1 = 2.97; // rads = 170 degrees for joints: 1 3 5 7
jlim2 = 2.09; // rads = 120 degrees for joints: 2 4 6
config_fk = 0;
n_joints = 7;
psi_fk = 0.0;
dbs=0.31;
dse=0.4;
dew=0.39;
dwt=0.078;
// q_ik = std::vector<double>(7, 0.0);
vbs = tf::Vector3(0.0,0.0,dbs);
vwt = tf::Vector3(0.0,0.0,dwt);
};
void leatherman::transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e)
{
e(0,3) = k.p[0];
e(1,3) = k.p[1];
e(2,3) = k.p[2];
e(0,0) = k.M(0,0);
e(0,1) = k.M(0,1);
e(0,2) = k.M(0,2);
e(1,0) = k.M(1,0);
e(1,1) = k.M(1,1);
e(1,2) = k.M(1,2);
e(2,0) = k.M(2,0);
e(2,1) = k.M(2,1);
e(2,2) = k.M(2,2);
e(3,0) = 0.0;
e(3,1) = 0.0;
e(3,2) = 0.0;
e(3,3) = 1.0;
}
void UrdfModelMarker::graspPointCB( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
//linkMarkerMap[feedback->marker_name].gp.pose = feedback->pose;
//publishMarkerPose(feedback);
//publishMarkerMenu(feedback, jsk_interactive_marker::MarkerMenu::MOVE);
KDL::Vector graspVec(feedback->mouse_point.x, feedback->mouse_point.y, feedback->mouse_point.z);
KDL::Frame parentFrame;
tf::poseMsgToKDL (linkMarkerMap[feedback->marker_name].pose, parentFrame);
graspVec = parentFrame.Inverse(graspVec);
geometry_msgs::Pose p;
p.position.x = graspVec.x();
p.position.y = graspVec.y();
p.position.z = graspVec.z();
p.orientation = linkMarkerMap[feedback->marker_name].gp.pose.orientation;
linkMarkerMap[feedback->marker_name].gp.pose = p;
linkMarkerMap[feedback->marker_name].gp.displayGraspPoint = true;
addChildLinkNames(model->getRoot(), true, false);
//addChildLinkNames(model->getRoot(), true, false, true, 0);
}
TEST(TFKDLConversions, tf_kdl_pose)
{
tf::Pose t;
tf::Quaternion tq;
tq[0] = gen_rand(-1.0,1.0);
tq[1] = gen_rand(-1.0,1.0);
tq[2] = gen_rand(-1.0,1.0);
tq[3] = gen_rand(-1.0,1.0);
tq.normalize();
t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
t.setRotation(tq);
//test tf->kdl
KDL::Frame k;
PoseTFToKDL(t,k);
for(int i=0; i < 3; i++)
{
ASSERT_NEAR(t.getOrigin()[i],k.p[i],1e-6);
for(int j=0; j < 3; j++)
{
ASSERT_NEAR(t.getBasis()[i][j],k.M(i,j),1e-6);
}
}
//test kdl->tf
tf::Pose r;
PoseKDLToTF(k,r);
for(int i=0; i< 3; i++)
{
ASSERT_NEAR(t.getOrigin()[i],r.getOrigin()[i],1e-6);
for(int j=0; j < 3; j++)
{
ASSERT_NEAR(t.getBasis()[i][j],r.getBasis()[i][j],1e-6);
}
}
}
void DynamicSlidingModeControllerTaskSpace::set_marker(KDL::Frame x, int id)
{
msg_marker_.header.frame_id = "world";
msg_marker_.header.stamp = ros::Time();
msg_marker_.ns = "end_effector";
msg_marker_.id = id;
msg_marker_.type = visualization_msgs::Marker::SPHERE;
msg_marker_.action = visualization_msgs::Marker::ADD;
msg_marker_.pose.position.x = x.p(0);
msg_marker_.pose.position.y = x.p(1);
msg_marker_.pose.position.z = x.p(2);
msg_marker_.pose.orientation.x = 0.0;
msg_marker_.pose.orientation.y = 0.0;
msg_marker_.pose.orientation.z = 0.0;
msg_marker_.pose.orientation.w = 1.0;
msg_marker_.scale.x = 0.005;
msg_marker_.scale.y = 0.005;
msg_marker_.scale.z = 0.005;
msg_marker_.color.a = 1.0;
msg_marker_.color.r = 0.0;
msg_marker_.color.g = 1.0;
msg_marker_.color.b = 0.0;
}
void ChainKinematics::getQ(double* q, const double* qInit, const double (&t)[3], const double (&rot)[9]) const {
int i,j;
KDL::JntArray qInitJA(Njnt_);
for(i=0; i<Njnt_; i++)
qInitJA(i) = qInit[i];
KDL::Frame frame;
for(i=0; i<3; i++) {
frame.p(i) = t[i];
for(j=0; j<3; j++)
frame.M(i,j) = rot[i*3+j];
}
KDL::JntArray qJA;
if(iksolverPos_->CartToJnt(qInitJA, frame, qJA) < 0) {
std::cerr << "ERROR: [ChainKinematics][getQ] something went wrong during CartToJnt! Exiting!" << std::endl;
exit(-1);
}
for(i=0; i<Njnt_; i++)
q[i] = qJA(i);
}
void CollisionModel::addLinkContacts(double dist_range, const std::string &link_name, const pcl::PointCloud<pcl::PointNormal>::Ptr &res,
const KDL::Frame &T_W_L, const std::vector<ObjectModel::Feature > &ob_features,
const KDL::Frame &T_W_O) {
const double lambda = - std::log(0.01) / (dist_range * dist_range);
std::list<std::pair<int, double> > link_pt;
for (int poidx = 0; poidx < ob_features.size(); poidx++) {
double min_dist = dist_range + 1.0;
for (int pidx = 0; pidx < res->points.size(); pidx++) {
KDL::Vector p1(res->points[pidx].x, res->points[pidx].y, res->points[pidx].z);
const KDL::Vector &p2( ob_features[poidx].T_O_F_.p );
double dist = (T_W_L * p1 - T_W_O * p2).Norm();
if (dist < min_dist) {
min_dist = dist;
}
}
if (min_dist < dist_range) {
link_pt.push_back( std::make_pair(poidx, min_dist) );
}
}
if ( link_pt.size() > 0 ) {
link_models_map_[link_name].features_.resize( link_pt.size() );
std::vector<KDL::Frame > T_L_F_vec( link_pt.size() );
col_link_names_.push_back(link_name);
int fidx = 0;
KDL::Vector col_pt;
double sum_weight = 0.0;
for (std::list<std::pair<int, double> >::const_iterator it = link_pt.begin(); it != link_pt.end(); it++, fidx++) {
int poidx = it->first;
link_models_map_[link_name].features_[fidx].pc1 = ob_features[poidx].pc1_;
link_models_map_[link_name].features_[fidx].pc2 = ob_features[poidx].pc2_;
KDL::Frame T_W_F = T_W_O * ob_features[poidx].T_O_F_;
T_L_F_vec[fidx] = T_W_L.Inverse() * T_W_F;
double dist = it->second;
link_models_map_[link_name].features_[fidx].dist = dist;
double weight = std::exp(-lambda * dist * dist);
link_models_map_[link_name].features_[fidx].weight = weight;
col_pt = col_pt + weight * T_L_F_vec[fidx].p;
sum_weight += weight;
}
link_models_map_[link_name].T_L_C_ = KDL::Frame(col_pt / sum_weight);
for (int fidx = 0; fidx < link_models_map_[link_name].features_.size(); fidx++) {
link_models_map_[link_name].features_[fidx].T_C_F = link_models_map_[link_name].T_L_C_.Inverse() * T_L_F_vec[fidx];
link_models_map_[link_name].features_[fidx].weight /= sum_weight;
}
}
}
/**
*
* @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;
}
void kinematic_filter::setWorld_StanceFoot(const KDL::Frame& World_StanceFoot)
{
this->StanceFoot_World=World_StanceFoot.Inverse();
this->World_StanceFoot=World_StanceFoot;
}
bool idynChain2kdlChain(iCub::iDyn::iDynChain & idynChain,KDL::Chain & kdlChain,std::vector<std::string> link_names,std::vector<std::string> joint_names, std::string final_frame_name, std::string initial_frame_name, int max_links)
{
int n_links, i;
bool use_names;
n_links = idynChain.getN();
//In iDyn, the joints are only 1 DOF (not 0, not more)
int n_joints = idynChain.getN();
if(n_links <= 0 ) return false;
if( (int)link_names.size() < n_links || (int)joint_names.size() < n_joints ) {
use_names = false;
} else {
use_names = true;
}
KDL::Frame kdlFrame = KDL::Frame();
KDL::Frame kdl_H = KDL::Frame();
KDL::Joint kdlJoint = KDL::Joint();
kdlChain = KDL::Chain();
KDL::Segment kdlSegment = KDL::Segment();
KDL::RigidBodyInertia kdlRigidBodyInertia = KDL::RigidBodyInertia();
if( initial_frame_name.length() != 0 ) {
idynMatrix2kdlFrame(idynChain.getH0(),kdlFrame);
kdlSegment = KDL::Segment(initial_frame_name,KDL::Joint(initial_frame_name+"_joint",KDL::Joint::None),kdlFrame);
kdlChain.addSegment(kdlSegment);
}
for(i=0; i<n_links && i < max_links; i++)
{
//forgive him, as he does not know what is doing
iCub::iKin::iKinLink & link_current = idynChain[i];
//For the first link and the last link, take in account also H0 and HN
if ( i == n_links - 1) {
if( final_frame_name.length() == 0 ) {
idynMatrix2kdlFrame(idynChain.getHN(),kdl_H);
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset())*kdl_H;
} else {
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset());
}
} else {
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset());
}
bool ret = idynDynamicalParameters2kdlRigidBodyInertia(idynChain.getMass(i),idynChain.getCOM(i).subcol(0,3,3),idynChain.getInertia(i),kdlRigidBodyInertia);
if( !ret ) return false;
KDL::Joint jnt_idyn;
//\todo: joint can also be blocked at values different from 0.0
if( idynChain.isLinkBlocked(i) ) {
if( use_names ) {
kdlSegment = KDL::Segment(link_names[i],KDL::Joint(joint_names[i],KDL::Joint::None),kdlFrame,kdlRigidBodyInertia);
} else {
kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),kdlFrame,kdlRigidBodyInertia);
}
} else {
if( use_names ) {
kdlSegment = KDL::Segment(link_names[i],KDL::Joint(joint_names[i],KDL::Joint::RotZ),kdlFrame,kdlRigidBodyInertia);
} else {
kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),kdlFrame,kdlRigidBodyInertia);
}
}
kdlChain.addSegment(kdlSegment);
}
//if specified, add a final fake link
if( final_frame_name.length() != 0 ) {
idynMatrix2kdlFrame(idynChain.getHN(),kdlFrame);
kdlSegment = KDL::Segment(final_frame_name,KDL::Joint(final_frame_name+"_joint",KDL::Joint::None),kdlFrame);
kdlChain.addSegment(kdlSegment);
}
//Considering the H0 transformation
if( initial_frame_name.length() == 0 ) {
KDL::Chain new_chain;
KDL::Frame kdl_H0;
idynMatrix2kdlFrame(idynChain.getH0(),kdl_H0);
//std::cout << "KDL_h0 " << kdl_H0 << std::endl;
addBaseTransformation(kdlChain,new_chain,kdl_H0);
kdlChain = new_chain;
}
return true;
}
void RobotModel::setKinematicsToPlanningTransform(const KDL::Frame &f, std::string name)
{
T_kinematics_to_planning_ = f;
T_planning_to_kinematics_ = f.Inverse();
planning_frame_ = name;
}
bool idynSensorChain2kdlChain(iCub::iDyn::iDynChain & idynChain,
iCub::iDyn::iDynInvSensor & idynSensor,
KDL::Chain & kdlChain,
KDL::Frame & H_sensor_dh_child,
std::vector<std::string> link_names,std::vector<std::string> joint_names, std::string final_frame_name, std::string initial_frame_name, int max_links)
{
bool use_names;
int n_links, i, sensor_link;
n_links = idynChain.getN();
sensor_link = idynSensor.getSensorLink();
int kdl_links = n_links + 1; //The sensor links transform a link in two different links
int kdl_joints = kdl_links;
if(n_links <= 0 ) return false;
if( (int)link_names.size() < kdl_links || (int)joint_names.size() < kdl_joints ) {
use_names = false;
} else {
use_names = true;
}
KDL::Frame kdlFrame = KDL::Frame();
KDL::Frame kdl_H;
kdlChain = KDL::Chain();
KDL::Segment kdlSegment = KDL::Segment();
KDL::RigidBodyInertia kdlRigidBodyInertia = KDL::RigidBodyInertia();
int kdl_i = 0;
if( initial_frame_name.length() != 0 ) {
idynMatrix2kdlFrame(idynChain.getH0(),kdlFrame);
kdlSegment = KDL::Segment(initial_frame_name,KDL::Joint(initial_frame_name+"_joint",KDL::Joint::None),kdlFrame);
kdlChain.addSegment(kdlSegment);
}
KDL::Frame remainder_frame = KDL::Frame::Identity();
for(i=0; i<n_links; i++)
{
if( i != sensor_link ) {
//forgive him, as he does not know what is doing
iCub::iKin::iKinLink & link_current = idynChain[i];
//For the last link, take in account also HN
if ( i == n_links - 1) {
idynMatrix2kdlFrame(idynChain.getHN(),kdl_H);
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset())*kdl_H;
} else {
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset());
}
bool ret = idynDynamicalParameters2kdlRigidBodyInertia(idynChain.getMass(i),idynChain.getCOM(i).subcol(0,3,3),idynChain.getInertia(i),kdlRigidBodyInertia);
assert(ret);
if(!ret) return false;
//For the joint after the ft sensor, consider the offset between the ft sensor
// and the joint
if( idynChain.isLinkBlocked(i) ) {
// if it is blocked, it is easy to add the remainder frame
if( use_names ) {
kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::None),remainder_frame*kdlFrame,kdlRigidBodyInertia);
kdl_i++;
} else {
kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),remainder_frame*kdlFrame,kdlRigidBodyInertia);
}
} else {
KDL::Vector new_joint_axis, new_joint_origin;
new_joint_axis = remainder_frame.M*KDL::Vector(0.0,0.0,0.1);
new_joint_origin = remainder_frame.p;
if( use_names ) {
kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],new_joint_origin,new_joint_axis,KDL::Joint::RotAxis),remainder_frame*kdlFrame,kdlRigidBodyInertia);
kdl_i++;
} else {
kdlSegment = KDL::Segment(KDL::Joint(new_joint_origin,new_joint_axis,KDL::Joint::RotAxis),remainder_frame*kdlFrame,kdlRigidBodyInertia);
}
}
kdlChain.addSegment(kdlSegment);
remainder_frame = KDL::Frame::Identity();
} else {
//( i == segment_link )
double m,m0,m1;
yarp::sig::Vector r0(3),r1(3),r(3),rgg0(3),rgg1(3),r_i_s_wrt_i(3),r_i_C0_wrt_i;
yarp::sig::Matrix I,I0,I1;
yarp::sig::Matrix R_s_wrt_i;
iCub::iKin::iKinLink & link_current = idynChain[sensor_link];
KDL::Frame kdlFrame_0 = KDL::Frame();
KDL::Frame kdlFrame_1 = KDL::Frame();
//Imagine that we have i, s , i+1
//yarp::sig::Matrix H_0;
//The angle of the sensor link joint is put to 0 and then restored
double angSensorLink = link_current.getAng();
yarp::sig::Matrix H_sensor_link = (link_current.getH(0.0)); //H_{i-1}_i
link_current.setAng(angSensorLink);
//idynSensor.getH() <--> H_i_s
yarp::sig::Matrix H_0 = H_sensor_link * (idynSensor.getH()); // H_{i-1}_s = H_{i-1}_i*H_i_s ?
yarp::sig::Matrix H_1 = localSE3inv(idynSensor.getH()); //H_s_{i}
//std::cout << "H_0" << std::endl << H_0.toString() << std::endl;
//std::cout << "H_1" << std::endl << H_1.toString() << std::endl;
idynMatrix2kdlFrame(H_0,kdlFrame_0);
idynMatrix2kdlFrame(H_1,kdlFrame_1);
remainder_frame = kdlFrame_1;
H_sensor_dh_child = remainder_frame;
kdlFrame_1 = KDL::Frame::Identity();
//cout << "kdlFrame_0: " << endl;
//cout << kdlFrame_0;
//cout << "kdlFrame_1: " << endl;
//cout << kdlFrame_1;
KDL::RigidBodyInertia kdlRigidBodyInertia_0 = KDL::RigidBodyInertia();
KDL::RigidBodyInertia kdlRigidBodyInertia_1 = KDL::RigidBodyInertia();
m = idynChain.getMass(sensor_link);
m1 = idynSensor.getMass();
m0 = m-m1;
//It is not possible that the semilink is more heavy than the link!!!
assert(m0 > 0);
//r_{i,C_i}^i
r = idynChain.getCOM(i).subcol(0,3,3);
//R_s^i
R_s_wrt_i = idynSensor.getH().submatrix(0,2,0,2);
r_i_s_wrt_i = idynSensor.getH().subcol(0,3,3);
//r0 := r_{s,C_{{vl}_0}}^s
//r1 := r_{i,C_{{vl}_1}}^i
//
r1 = r_i_s_wrt_i + R_s_wrt_i*(idynSensor.getCOM().subcol(0,3,3));
//cout << "m0: " << m0 << endl;
r_i_C0_wrt_i = (1/m0)*(m*r-m1*r1);
r0 = R_s_wrt_i.transposed()*(r_i_C0_wrt_i-r_i_s_wrt_i);
I = idynChain.getInertia(i);
I1 = R_s_wrt_i*idynSensor.getInertia()*R_s_wrt_i.transposed();
rgg0 = -1*r+r_i_C0_wrt_i;
rgg1 = -1*r+r1;
I0 = R_s_wrt_i.transposed()*(I - I1 + m1*crossProductMatrix(rgg1)*crossProductMatrix(rgg1) + m0*crossProductMatrix(rgg0)*crossProductMatrix(rgg0))*R_s_wrt_i;
//DEBUG
//printMatrix("I",I);
//printMatrix("I1",I1);
//printMatrix("I0",I0);
//printMatrix("cross",crossProductMatrix(rgg1));
//cout << "m0: " << m0 << endl;
//printVector("r0",r0);
//printMatrix("I0",I0);
bool ret;
ret = idynDynamicalParameters2kdlRigidBodyInertia(m0,r0,I0,kdlRigidBodyInertia_0);
assert(ret);
if(!ret) return false;
ret = idynDynamicalParameters2kdlRigidBodyInertia(m1,r1,I1,kdlRigidBodyInertia_1);
assert(ret);
if(!ret) return false;
KDL::RigidBodyInertia kdlRigidBodyInertia_1_buf;
kdlRigidBodyInertia_1_buf = remainder_frame*kdlRigidBodyInertia_1;
kdlRigidBodyInertia_1 = kdlRigidBodyInertia_1_buf;
if( use_names ) {
kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::RotZ),kdlFrame_0,kdlRigidBodyInertia_0);
kdl_i++;
} else {
kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),kdlFrame_0,kdlRigidBodyInertia_0);
}
kdlChain.addSegment(kdlSegment);
if( use_names ) {
kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::None),kdlFrame_1,kdlRigidBodyInertia_1);
kdl_i++;
} else {
kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),kdlFrame_1,kdlRigidBodyInertia_1);
}
kdlChain.addSegment(kdlSegment);
}
}
//Final link can be segment
if( final_frame_name.length() != 0 ) {
idynMatrix2kdlFrame(idynChain.getHN(),kdlFrame);
kdlSegment = KDL::Segment(final_frame_name,KDL::Joint(final_frame_name+"_joint",KDL::Joint::None),kdlFrame);
kdlChain.addSegment(kdlSegment);
}
if( max_links < (int)kdlChain.getNrOfSegments() )
{
KDL::Chain new_kdlChain;
for(int p=0; p < max_links; p++)
{
new_kdlChain.addSegment(kdlChain.getSegment(p));
}
kdlChain = new_kdlChain;
}
//Considering the H0 transformation
if( initial_frame_name.length() == 0 ) {
KDL::Chain new_chain;
KDL::Frame kdl_H0;
idynMatrix2kdlFrame(idynChain.getH0(),kdl_H0);
addBaseTransformation(kdlChain,new_chain,kdl_H0);
kdlChain = new_chain;
}
return true;
}
bool WamIkKdl::ik(vector<double> goal_in_cartesian, vector<double> currentjoints, vector<double>& goal_in_joints){
//fk solver
KDL::ChainFkSolverPos_recursive fksolver(KDL::ChainFkSolverPos_recursive(this->wam63_));
// Create joint array
KDL::JntArray setpointJP = KDL::JntArray(this->num_joints_);
KDL::JntArray max = KDL::JntArray(this->num_joints_); //The maximum joint positions
KDL::JntArray min = KDL::JntArray(this->num_joints_); //The minimium joint positions
double minjp[7] = {-2.6,-2.0,-2.8,-0.9,-4.76,-1.6,-3.0};
double maxjp[7] = { 2.6, 2.0, 2.8, 3.2, 1.24, 1.6, 3.0};
for(unsigned int ii=0; ii < this->num_joints_; ii++){
max(ii) = maxjp[ii];
min(ii) = minjp[ii];
}
//Create inverse velocity solver
KDL::ChainIkSolverVel_pinv_givens iksolverv = KDL::ChainIkSolverVel_pinv_givens(this->wam63_);
//Iksolver Position: Maximum 100 iterations, stop at accuracy 1e-6
//ChainIkSolverPos_NR iksolver = ChainIkSolverPos_NR(wam63,fksolver,iksolverv,100,1e-6);
KDL::ChainIkSolverPos_NR_JL iksolver = KDL::ChainIkSolverPos_NR_JL(this->wam63_, min, max, fksolver, iksolverv, 2000, 1e-6); //With Joints Limits
KDL::Frame cartpos;
KDL::JntArray jointpos = KDL::JntArray(this->num_joints_);
KDL::JntArray currentJP = KDL::JntArray(this->num_joints_);
// Copying position to KDL frame
cartpos.p(0) = goal_in_cartesian.at(3);
cartpos.p(1) = goal_in_cartesian.at(7);
cartpos.p(2) = goal_in_cartesian.at(11);
// Copying Rotation to KDL frame
cartpos.M(0,0) = goal_in_cartesian.at(0);
cartpos.M(0,1) = goal_in_cartesian.at(1);
cartpos.M(0,2) = goal_in_cartesian.at(2);
cartpos.M(1,0) = goal_in_cartesian.at(4);
cartpos.M(1,1) = goal_in_cartesian.at(5);
cartpos.M(1,2) = goal_in_cartesian.at(6);
cartpos.M(2,0) = goal_in_cartesian.at(8);
cartpos.M(2,1) = goal_in_cartesian.at(9);
cartpos.M(2,2) = goal_in_cartesian.at(10);
for(unsigned int ii=0; ii < this->num_joints_; ii++)
currentJP(ii) = currentjoints.at(ii);
// Calculate inverse kinematics to go from currentJP to cartpos. The result in jointpos
int ret = iksolver.CartToJnt(currentJP, cartpos, jointpos);
if (ret >= 0) {
std::cout << " Current Joint Position: [";
for(unsigned int ii=0; ii < this->num_joints_; ii++)
std::cout << currentJP(ii) << " ";
std::cout << "]"<< std::endl;
std::cout << "Cartesian Position " << cartpos << std::endl;
std::cout << "IK result Joint Position: [";
for(unsigned int ii=0; ii < this->num_joints_; ii++)
std::cout << jointpos(ii) << " ";
std::cout << "]"<< std::endl;
goal_in_joints.clear();
goal_in_joints.resize(this->num_joints_);
for(unsigned int ii=0; ii < this->num_joints_; ii++)
goal_in_joints[ii] = jointpos(ii);
} else {
std::cout << "Error: could not calculate inverse kinematics :(" << std::endl;
return false;
}
return true;
}