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