Exemple #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);
	}

}
Exemple #2
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);

};
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;
        }
    }
}
Exemple #4
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 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;
}