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);
}
}
//------------------------------------------------------------------------------
// 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 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 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);
}
void kinematic_filter::setWorld_StanceFoot(const KDL::Frame& World_StanceFoot)
{
this->StanceFoot_World=World_StanceFoot.Inverse();
this->World_StanceFoot=World_StanceFoot;
}
void RobotModel::setKinematicsToPlanningTransform(const KDL::Frame &f, std::string name)
{
T_kinematics_to_planning_ = f;
T_planning_to_kinematics_ = f.Inverse();
planning_frame_ = name;
}
