Example #1
0
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;	
	}
Example #3
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;
 }
Example #6
0
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);
}
Example #7
0
//------------------------------------------------------------------------------
// 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);

};
Example #8
0
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);
}
Example #10
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;	
	}
Example #12
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);
}
Example #13
0
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;
        }
    }
}
Example #14
0
/**
 *
 * @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;
}
Example #16
0
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;
}
Example #17
0
void RobotModel::setKinematicsToPlanningTransform(const KDL::Frame &f, std::string name)
{
  T_kinematics_to_planning_ = f;
  T_planning_to_kinematics_ = f.Inverse();
  planning_frame_ = name;
}
Example #18
0
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;
}
Example #19
0
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;
}