void getKDLChainInfo(kinematics_msgs::KinematicSolverInfo &chain_info)
{
	unsigned int nj = chain.getNrOfJoints();
	unsigned int nl = chain.getNrOfSegments();
	
	ROS_DEBUG("nj: %d", nj);
	ROS_DEBUG("nl: %d", nl);

	//---setting up response

	//joint_names
	for(unsigned int i=0; i<nj; i++)
	{
		ROS_DEBUG("joint_name[%d]: %s", i, chain.getSegment(i).getJoint().getName().c_str());
		chain_info.joint_names.push_back(chain.getSegment(i).getJoint().getName());
	}
	//limits
		//joint limits are only saved in KDL::ChainIkSolverPos_NR_JL iksolverpos -> ik_solve().....but this is not visible here!
/*for(unsigned int i=0; i<nj; i++)
  {
		chain_info.limits[i].joint_name=chain.getSegment(i).getJoint().getName();
		chain_info.limits[i].has_position_limits=
		chain_info.limits[i].min_position=
		chain_info.limits[i].max_position=	
		chain_info.limits[i].has_velocity_limits=
		chain_info.limits[i].max_velocity=
		chain_info.limits[i].has_acceleration_limits=
		chain_info.limits[i].max_acceleration=
	}*/
	//link_names	
	for(unsigned int i=0; i<nl; i++)
	{
		chain_info.link_names.push_back(chain.getSegment(i).getName());
	}
}
Exemple #2
0
    // Get Joint Limits from an URDF model
    void getJointLimitsFromModel(const urdf::Model& model,
                                 std::vector<double> &q_min,
                                 std::vector<double> &q_max,                                 
                                 KDL::Chain &kdlArmChain
                                ) 
    {
        boost::shared_ptr<const urdf::Joint> joint;

        for(unsigned int i=0; i< kdlArmChain.getNrOfJoints(); ++i)
        {
            if (kdlArmChain.getSegment(i).getJoint().getType() != KDL::Joint::None)
            {
                joint = model.getJoint(kdlArmChain.getSegment(i).getJoint().getName());
                if ( joint->type != urdf::Joint::CONTINUOUS )
                {
                    q_min.at(i) = joint->limits->lower;
                    q_max.at(i) = joint->limits->upper;
                }
                else
                {
                    q_min.at(i) = -3.14/2.0;
                    q_max.at(i) = 3.14/2.0;
                }
            }
        }
    }
Exemple #3
0
ChainKinematics::ChainKinematics(const KDL::Chain chain, const double lambda) : 
	kinChain_(chain),
	Njnt_(chain.getNrOfJoints()),
	Nsegment_(chain.getNrOfSegments()) {

		initialize(lambda);

	}
KDL::Wrenches idynChainGet_usingKDL_aux(iCub::iDyn::iDynChain & idynChain, iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
    yarp::sig::Vector q,dq,ddq;
    q = idynChain.getAng();
    dq = idynChain.getDAng();
    ddq = idynChain.getD2Ang();
    
    KDL::Chain kdlChain;
    
    std::vector<std::string> la_joints;
    la_joints.push_back("left_shoulder_pitch");
    la_joints.push_back("left_shoulder_roll");
    la_joints.push_back("left_arm_ft_sensor");
    la_joints.push_back("left_shoulder_yaw");
    la_joints.push_back("left_elbow");
    la_joints.push_back("left_wrist_prosup");
    la_joints.push_back("left_wrist_pitch");
    la_joints.push_back("left_wrist_yaw");


    
    idynSensorChain2kdlChain(idynChain,idynSensor,kdlChain,la_joints,la_joints);
    
    std::cout << kdlChain << std::endl;
    
    int nj = idynChain.getN();
    
    //cout << "idynChainGet_usingKDL_aux with sensor"  << " nrJoints " << kdlChain.getNrOfJoints() <<  " nrsegments " << kdlChain.getNrOfSegments() << endl;

    assert(nj==kdlChain.getNrOfJoints());
    assert(nj+1==kdlChain.getNrOfSegments());
    
    
    KDL::JntArray jointpositions = KDL::JntArray(nj);
    KDL::JntArray jointvel = KDL::JntArray(nj);
    KDL::JntArray jointacc = KDL::JntArray(nj);
    KDL::JntArray torques = KDL::JntArray(nj);

    for(unsigned int i=0;i<nj;i++){
        jointpositions(i)=q[i];
        jointvel(i) = dq[i];
        jointacc(i) = ddq[i];
    }
    
    KDL::Wrenches f_ext(nj+1);
    KDL::Wrenches f_int(nj+1);
    
    KDL::Vector grav_kdl;
    idynVector2kdlVector(idynChain.getH0().submatrix(0,2,0,2).transposed()*ddp0,grav_kdl);
    
    KDL::ChainIdSolver_RNE_IW neSolver = KDL::ChainIdSolver_RNE_IW(kdlChain,-grav_kdl);
    
    int ret = neSolver.CartToJnt_and_internal_wrenches(jointpositions,jointvel,jointacc,f_ext,torques,f_int);
    assert(ret >= 0);
    
    return f_int;
}
 void getKDLChainInfo(const KDL::Chain &chain,
                      kinematics_msgs::KinematicSolverInfo &chain_info)
 {
   int i=0; // segment number
   while(i < (int)chain.getNrOfSegments())
   {
     chain_info.link_names.push_back(chain.getSegment(i).getName());
     i++;
   }
 }
int Kinematics::getKDLSegmentIndex(const std::string &name) {
    int i=0; 
    while (i < (int)chain.getNrOfSegments()) {
        if (chain.getSegment(i).getName() == name) {
            return i+1;
        }
        i++;
    }
    return -1;
}
int ExcavaROBArmKinematics::getKDLSegmentIndex(const std::string &name) {
    int i=0; 
    while (i < (int) arm_chain_.getNrOfSegments()) {
        if (arm_chain_.getSegment(i).getName() == name) {
            return i + 1;
        }
        i++;
    }
    return -1;
}
Exemple #8
0
sensor_msgs::JointState init_message(const KDL::Chain &chain) {
	sensor_msgs::JointState msg;
	for (unsigned int i=0; i < chain.getNrOfSegments(); ++i) {
		KDL::Segment segment = chain.getSegment(i);
		KDL::Joint joint = segment.getJoint();
		if (joint.getType() == KDL::Joint::JointType::None) continue;
		msg.name.push_back(joint.getName());
		msg.position.push_back(0);
	}
	return msg;
}
FkLookup::ChainFK::ChainFK(const KDL::Tree& tree, const std::string& root, const std::string& tip):root_name(root),tip_name(tip){
    KDL::Chain chain;
    tree.getChain(root_name, tip_name, chain);
    solver =  new KDL::ChainFkSolverPos_recursive(chain);
    unsigned int  num = chain.getNrOfSegments();
    for(unsigned int i = 0; i < num; ++i){
	const KDL::Joint &joint = chain.getSegment(i).getJoint();
	if (joint.getType() != KDL::Joint::None) joints.insert(std::make_pair(joint.getName(),joints.size()));
    }
    ROS_ASSERT(joints.size() == chain.getNrOfJoints());
}
Exemple #10
0
void printKDLchain(std::string s,const KDL::Chain & kdlChain)
{
    std::cout << s << std::endl;
    for(int p=0; p < (int)kdlChain.getNrOfSegments(); p++)
    {
        std::cout << "Segment " << p << ":" << std::endl;
        KDL::Segment seg = kdlChain.getSegment(p);
        KDL::Frame fra = seg.getFrameToTip();
        std::cout << seg << std::endl;
        std::cout << fra << std::endl;
    }
}
Exemple #11
0
bool leatherman::getSegmentIndex(const KDL::Chain &c, std::string name, int &index)
{
  for(size_t j = 0; j < c.getNrOfSegments(); ++j)
  {
    if(c.getSegment(j).getName().compare(name) == 0)
    {
      index = j;
      return true;
    }
  }
  return false;
}
KDL::Chain kukaControl::LWR_knife() {
	KDL::Chain chain;

	// base
	chain.addSegment(
	// KDL::Segment(KDL::Joint(KDL::Joint::None),KDL::Frame::DH_Craig1989(0, 0, 0.33989, 0)));
			KDL::Segment(KDL::Joint(KDL::Joint::None),
					KDL::Frame::DH_Craig1989(0, 0, 0.34, 0)));
	// joint 1
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
	// joint 2
	chain.addSegment(
	// KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40011, 0)));
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40, 0)));
	// joint 3
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));
	// joint 4
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0.4000, 0)));
	// joint 5
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
	// joint 6
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));

	// joint 7 (with flange adapter)
	/*  chain.addSegment(
	 KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
	 KDL::Frame::DH_Craig1989(0, 0, 0.12597, 0)));*/
	// for HSC
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, 0, 0.12597 + 0.41275, 0)));

	KDL::Frame ee = KDL::Frame(KDL::Vector(0.0625, -0.0925, -0.07))
			* KDL::Frame(
					KDL::Rotation::EulerZYX(32.0 * M_PI / 180, 0.0,
							54.18 * M_PI / 180));
	chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None), ee));

	return chain;

}
Exemple #13
0
IkSolver::IkSolver(const KDL::Chain& chain)
{
  const std::string root_name = chain.getSegment(0).getName();
  const std::string tip_name  = chain.getSegment(chain.getNrOfSegments() - 1).getName();

  KDL::Tree tree("root");
  tree.addChain(chain, "root");

  std::vector<std::string> endpoint_names(1, tip_name);
  std::vector<EndpointCoupling> endpoint_couplings(1, Pose);

  init(tree, endpoint_names, endpoint_couplings);
}
 int getKDLSegmentIndex(const KDL::Chain &chain, 
                        const std::string &name)
 {
   int i=0; // segment number
   while(i < (int)chain.getNrOfSegments())
   {
     if(chain.getSegment(i).getName() == name)
     {
       return i+1;
     }
     i++;
   }
   return -1;   
 }
void computeRNEDynamicsForChain(KDL::Tree& a_tree, const std::string& rootLink, const std::string& tipLink, KDL::Vector& grav,
                                std::vector<kdle::JointState>& jointState, std::vector<kdle::SegmentState>& linkState)
{
    KDL::Chain achain;
    a_tree.getChain(rootLink, tipLink, achain);

    KDL::JntArray q(achain.getNrOfJoints());
    KDL::JntArray q_dot(achain.getNrOfJoints());
    KDL::JntArray q_dotdot(achain.getNrOfJoints());
    JntArray torques(achain.getNrOfJoints());
    KDL::Wrenches f_ext;
    f_ext.resize(achain.getNrOfSegments());

    std::cout << endl << endl;
    printf("RNE dynamics values \n");

    KDL::ChainIdSolver_RNE *rneDynamics = new ChainIdSolver_RNE(achain, -grav);
    
    for (unsigned int i = 0; i < achain.getNrOfJoints(); ++i)
    {
        q(i) = jointState[i].q;
        q_dot(i) = jointState[i].qdot;
        q_dotdot(i) = jointState[i].qdotdot;
        printf("q, qdot %f, %f\n", q(i), q_dot(i));
    }

    
    rneDynamics->CartToJnt(q, q_dot, q_dotdot, f_ext, torques);

    for (unsigned int i = 0; i < achain.getNrOfJoints(); ++i)
    {
        printf("index, q, qdot, torques %d, %f, %f, %f\n", i, q(i), q_dot(i), torques(i));
    }
    return;
}
void RobotAlgos::Test(void)
{
	    //Definition of a kinematic chain & add segments to the chain
    KDL::Chain chain;
    chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
    chain.addSegment(Segment(Joint(Joint::RotZ)));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
    chain.addSegment(Segment(Joint(Joint::RotZ)));
 
    // Create solver based on kinematic chain
    ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
 
    // Create joint array
    unsigned int nj = chain.getNrOfJoints();
    KDL::JntArray jointpositions = JntArray(nj);
 
    // Assign some values to the joint positions
    for(unsigned int i=0;i<nj;i++){
        float myinput;
        printf ("Enter the position of joint %i: ",i);
        scanf ("%e",&myinput);
        jointpositions(i)=(double)myinput;
    }
 
    // Create the frame that will contain the results
    KDL::Frame cartpos;    
 
    // Calculate forward position kinematics
    int kinematics_status;
    kinematics_status = fksolver.JntToCart(jointpositions,cartpos);
    if(kinematics_status>=0){
        std::cout << cartpos <<std::endl;
        printf("%s \n","Succes, thanks KDL!");
    }else{
        printf("%s \n","Error: could not calculate forward kinematics :(");
    }


	//Creation of the solvers:
	ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
	ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
	ChainIkSolverPos_NR iksolver1(chain,fksolver1,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6
	 
	//Creation of jntarrays:
	JntArray result(chain.getNrOfJoints());
	JntArray q_init(chain.getNrOfJoints());
	 
	//Set destination frame
	Frame F_dest=cartpos;
	 
	iksolver1.CartToJnt(q_init,F_dest,result);

	for(unsigned int i=0;i<nj;i++){
        printf ("Axle %i: %f \n",i,result(i));
    }


}
Exemple #17
0
int main()
{
    KDLCollada kdlCollada;
    vector <KDL::Chain> kinematicsModels;
    const string filename =  "puma.dae";  // loading collada kinematics model and converting it to kdl serial chain
    if (!kdlCollada.load(COLLADA_MODELS_PATH + filename, kinematicsModels))
    {
        cout << "Failed to import " << filename;
        return 0;
    }

    cout << "Imported " << kinematicsModels.size() << " kinematics chains" << endl;

    for (unsigned int i = 0; i < kinematicsModels.size(); i++)  // parsing output kdl serail chain
    {
        KDL::Chain chain = kinematicsModels[i];
        cout << "Chain " << i << " has " << chain.getNrOfSegments() << " segments" << endl;

        for (unsigned int u = 0; u < chain.getNrOfSegments(); u++)
        {
            KDL::Segment segment = chain.segments[u];
            string segmentName = segment.getName();
            cout << "Segment " << segmentName << " :" <<endl;

            KDL::Frame f_tip = segment.getFrameToTip();
            KDL::Vector rotAxis = f_tip.M.GetRot();
            double rotAngle = f_tip.M.GetRotAngle(rotAxis);
            KDL::Vector trans = f_tip.p;
            cout << "   frame: rotation " << rotAxis.x() << " " << rotAxis.y() << " " << rotAxis.z() << " " << rotAngle * 180 / M_PI << endl;
            cout << "   frame: translation " << trans.x() << " " << trans.y() << " " << trans.z() << endl;

            KDL::RigidBodyInertia inertia = segment.getInertia();
            KDL::Joint joint = segment.getJoint();

            string jointName = joint.getName();
            string jointType = joint.getTypeName();
            KDL::Vector jointAxis = joint.JointAxis();
            KDL::Vector jointOrigin = joint.JointOrigin();

            cout << "   joint name: " << jointName << endl;
            cout << "         type: " << jointType << endl;
            cout << "         axis: " << jointAxis.x() << " " <<jointAxis.y() << " " << jointAxis.z() << endl;
            cout << "         origin: " << jointOrigin.x() << " " << jointOrigin.y() << " " << jointOrigin.z() << endl;
        }
    }

    return 0;
}
Exemple #18
0
TEST_F(PR2IKTest, SingleRowRotation)
{
  YAML::Node node = YAML::LoadFile("pr2_left_arm_scope.yaml");

  ASSERT_NO_THROW(node.as< giskard_core::ScopeSpec >());
  giskard_core::ScopeSpec scope_spec = node.as<giskard_core::ScopeSpec>();

  ASSERT_NO_THROW(giskard_core::generate(scope_spec));
  giskard_core::Scope scope = giskard_core::generate(scope_spec);

  ASSERT_TRUE(scope.has_double_expression("pr2_rot_x"));

  KDL::Expression<double>::Ptr exp = scope.find_double_expression("pr2_rot_x");
  ASSERT_TRUE(exp.get()); 

  std::string base = "base_link";
  std::string tip = "l_wrist_roll_link";
  KDL::Chain chain;
  ASSERT_TRUE(tree.getChain(base, tip, chain));
  ASSERT_EQ(chain.getNrOfJoints(), exp->number_of_derivatives());

  boost::shared_ptr<KDL::ChainJntToJacSolver> jac_solver;
  jac_solver = boost::shared_ptr<KDL::ChainJntToJacSolver>(
      new KDL::ChainJntToJacSolver(chain));

  for(int i=-11; i<12; ++i)
  {
    std::vector<double> exp_in;
    KDL::JntArray solver_in(exp->number_of_derivatives());
    for(size_t j=0; j<exp->number_of_derivatives(); ++j)
    {
      double value = 0.1*i;
      exp_in.push_back(value);
      solver_in(j) = value;
    }

    exp->setInputValues(exp_in);
    exp->value();

    KDL::Jacobian solver_jac(chain.getNrOfJoints());
    ASSERT_GE(jac_solver->JntToJac(solver_in, solver_jac), 0.0);

    for (size_t j=0; j<chain.getNrOfJoints(); ++j)
      EXPECT_NEAR(solver_jac(3,j), exp->derivative(j), KDL::epsilon);
  }
}
Exemple #19
0
TEST_F(PR2IKTest, SingleExpression)
{
  YAML::Node node = YAML::LoadFile("pr2_left_arm_single_expression.yaml");
  ASSERT_NO_THROW(node.as<giskard_core::FrameSpecPtr>());

  giskard_core::FrameSpecPtr spec = node.as<giskard_core::FrameSpecPtr>();
  ASSERT_NO_THROW(spec->get_expression(giskard_core::Scope()));
  
  KDL::Expression<KDL::Frame>::Ptr exp = spec->get_expression(giskard_core::Scope());
  ASSERT_TRUE(exp.get()); 

  std::string base = "torso_lift_link";
  std::string tip = "l_wrist_roll_link";
  KDL::Chain chain;
  ASSERT_TRUE(tree.getChain(base, tip, chain));
  ASSERT_EQ(chain.getNrOfJoints(), exp->number_of_derivatives());

  boost::shared_ptr<KDL::ChainJntToJacSolver> jac_solver;
  jac_solver = boost::shared_ptr<KDL::ChainJntToJacSolver>(
      new KDL::ChainJntToJacSolver(chain));

  for(int i=-11; i<12; ++i)
  {
    std::vector<double> exp_in;
    KDL::JntArray solver_in(exp->number_of_derivatives());
    for(size_t j=0; j<exp->number_of_derivatives(); ++j)
    {
      double value = 0.1*i;
      exp_in.push_back(value);
      solver_in(j) = value;
    }

    exp->setInputValues(exp_in);
    exp->value();
    KDL::Jacobian exp_jac(exp->number_of_derivatives());
    for(size_t j=0; j<exp_jac.columns(); ++j)
      exp_jac.setColumn(j, exp->derivative(j));

    KDL::Jacobian solver_jac(chain.getNrOfJoints());
    ASSERT_GE(jac_solver->JntToJac(solver_in, solver_jac), 0.0);

    EXPECT_TRUE(KDL::Equal(solver_jac, exp_jac));
  }
}
KDL::Chain kukaControl::LWR_HSC_nozzle_RCM_used() {
	KDL::Chain chain;
	// base
	chain.addSegment(
	// KDL::Segment(KDL::Joint(KDL::Joint::None),KDL::Frame::DH_Craig1989(0, 0, 0.33989, 0)));
			KDL::Segment(KDL::Joint(KDL::Joint::None),
					KDL::Frame::DH_Craig1989(0, 0, 0.34, 0)));
          //KDL::Frame::DH_Craig1989(0, M_PI_2, 0.36, 0)));
	// joint 1
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
	// joint 2
	chain.addSegment(
	// KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40011, 0)));
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40, 0)));
	          //KDL::Frame::DH_Craig1989(0, M_PI_2, 0.42, 0)));
	// joint 3
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));
	// joint 4
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0.4000, 0)));
	// joint 5
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
	// joint 6
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));

	// joint 7 (with flange adapter)
	/*  chain.addSegment(
	 KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
	 KDL::Frame::DH_Craig1989(0, 0, 0.12597, 0)));*/
	// for HSC
	chain.addSegment(
			KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
					KDL::Frame::DH_Craig1989(0, 0,
							0.12597 + Dist_from_EndeffectorToRcmPoint, 0)));

	return chain;

}
Exemple #21
0
void ArmKinematics::initInternals(const KDL::Chain& chain)
{
  dof_ = chain.getNrOfJoints();
  q_tmp_.resize(dof_);
  
  deleteInternals();

  fk_solver_ = new KDL::ChainFkSolverPos_recursive(chain);
  ik_solver_ = new KDL::ChainIkSolverVel_wdls(chain);
}
Exemple #22
0
bool addBaseTransformation(const KDL::Chain & old_chain, KDL::Chain & new_chain, KDL::Frame H_new_old)
{
    new_chain = KDL::Chain();
    for(unsigned int i=0; i<old_chain.getNrOfSegments(); i++) {
        KDL::Segment segm;
        segm = old_chain.getSegment(i);
        //if is not the first segment add normally the segment
        if( i != 0 ) {
            new_chain.addSegment(segm);
        } else {
            //otherwise modify the segment before adding it
            KDL::Segment new_segm;
            KDL::Joint new_joint, old_joint;
            old_joint = segm.getJoint();
            KDL::Joint::JointType new_type;
            switch(old_joint.getType()) {
            case KDL::Joint::RotAxis:
            case KDL::Joint::RotX:
            case KDL::Joint::RotY:
            case KDL::Joint::RotZ:
                new_type = KDL::Joint::RotAxis;
                break;
            case KDL::Joint::TransAxis:
            case KDL::Joint::TransX:
            case KDL::Joint::TransY:
            case KDL::Joint::TransZ:
                new_type = KDL::Joint::TransAxis;
                break;
            case KDL::Joint::None:
            default:
                new_type = KDL::Joint::None;
            }

            //check !

            new_joint = KDL::Joint(old_joint.getName(),H_new_old*old_joint.JointOrigin(),H_new_old.M*old_joint.JointAxis(),new_type);
            new_segm = KDL::Segment(segm.getName(),new_joint,H_new_old*segm.getFrameToTip(),segm.getInertia());
            new_chain.addSegment(new_segm);
        }
    }
    return true;
}
bool OrientationConstraint::initialize(const arm_navigation_msgs::OrientationConstraint& constraint,
                const KDL::Chain& chain)
{
  constraint_ = constraint;
  link_id_ = -1;
  for (unsigned int i=0; i<chain.getNrOfSegments(); ++i)
  {
    if (constraint_.link_name == chain.getSegment(i).getName())
    {
      link_id_ = i;
      break;
    }
  }
  if (link_id_ == -1)
  {
    ROS_ERROR("OrientationConstraint: couldn't find link %s in chain.", constraint_.link_name.c_str());
    return false;
  }
  return true;
}
bool ExcavaROBArmKinematics::getPositionFK(excavaROB_arm_kinematics_msgs::GetPositionFK::Request &request,
                                           excavaROB_arm_kinematics_msgs::GetPositionFK::Response &response) {
    KDL::Frame p_out;
    KDL::JntArray jnt_pos_in;
    geometry_msgs::PoseStamped pose;
    tf::Stamped<tf::Pose> tf_pose;

    if (num_joints_arm_ != request.fk_link_names.size()) {
	ROS_ERROR("The request has not the same dimension of the arm_chain joints.");
	return false;
    }

    jnt_pos_in.resize(num_joints_arm_);
    for (unsigned int i = 0; i < num_joints_arm_; i++) {
        int jnt_index = getJointIndex(request.joint_state.name[i]);
        if (jnt_index >= 0)
            jnt_pos_in(jnt_index) = request.joint_state.position[i];
    }

    response.pose_stamped.resize(num_joints_arm_);
    response.fk_link_names.resize(num_joints_arm_);

    bool valid = true;
    for (unsigned int i = 0; i < num_joints_arm_; i++) {
        int segmentIndex = getKDLSegmentIndex(request.fk_link_names[i]);
        ROS_DEBUG("End effector index: %d", segmentIndex);
        ROS_DEBUG("Chain indices: %d", arm_chain_.getNrOfSegments());

        if (fk_solver_->JntToCart(jnt_pos_in, p_out, segmentIndex) >= 0) {
            tf_pose.frame_id_ = root_name_;
            tf_pose.stamp_ = ros::Time();
            tf::PoseKDLToTF(p_out, tf_pose);
            try {
                tf_listener_.transformPose(request.header.frame_id, tf_pose, tf_pose);
            } catch (...) {
                ROS_ERROR("ExcavaROB Kinematics: Could not transform FK pose to frame: %s", request.header.frame_id.c_str());
                response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
                return false;
            }
            tf::poseStampedTFToMsg(tf_pose, pose);
            response.pose_stamped[i] = pose;
            response.fk_link_names[i] = request.fk_link_names[i];
            response.error_code.val = response.error_code.SUCCESS;
        } else {
            ROS_ERROR("ExcavaROB Kinematics: Could not compute FK for %s", request.fk_link_names[i].c_str());
            response.error_code.val = response.error_code.NO_FK_SOLUTION;
            valid = false;
        }
    }
    return true;
}
Exemple #25
0
bool fk_solve_TCP(kinematics_msgs::GetPositionFK::Request &req,
		 kinematics_msgs::GetPositionFK::Response &res )
{
	ROS_INFO("[TESTING]: get_fk_TCP_service has been called!");

	unsigned int nj = chain.getNrOfJoints();
	
	ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver

	JntArray conf(nj);
    geometry_msgs::PoseStamped pose;
    tf::Stamped<tf::Pose> tf_pose;

	for(int i = 0; i < nj; i++)
		conf(i) = req.robot_state.joint_state.position[i];
	Frame F_ist;
	res.pose_stamped.resize(1);
	res.fk_link_names.resize(1);
	if(fksolver1.JntToCart(conf, F_ist) >= 0)
	{
		std::cout << "Calculated Position out of Configuration:\n";
		std::cout << F_ist <<"\n";
	
		//TODO: fill out response!!!

        tf_pose.frame_id_ = "base_link";//root_name_;
        tf_pose.stamp_ = ros::Time();
        tf::PoseKDLToTF(F_ist,tf_pose);
        try
		{
          //tf_.transformPose(req.header.frame_id,tf_pose,tf_pose);
        }
        catch(...)
        {
          ROS_ERROR("Could not transform FK pose to frame: %s",req.header.frame_id.c_str());
          res.error_code.val = res.error_code.FRAME_TRANSFORM_FAILURE;
          return false;
        }
        tf::poseStampedTFToMsg(tf_pose,pose);
        res.pose_stamped[0] = pose;
        res.fk_link_names[0] = "arm_7_link";
        res.error_code.val = res.error_code.SUCCESS;
	}
	else
	{
        ROS_ERROR("Could not compute FK for arm_7_link");
        res.error_code.val = res.error_code.NO_FK_SOLUTION;
	}

	return true;
}
    VirtualForcePublisher()
    {
        ros::NodeHandle n_tilde("~");
        ros::NodeHandle n;

        // subscribe to joint state
        joint_state_sub_ = n.subscribe("joint_states", 1, &VirtualForcePublisher::callbackJointState, this);
        wrench_stamped_pub_ = n.advertise<geometry_msgs::WrenchStamped>("wrench", 1);

        // set publish frequency
        double publish_freq;
        n_tilde.param("publish_frequency", publish_freq, 50.0);
        publish_interval_ = ros::Duration(1.0/std::max(publish_freq,1.0));

        //set time constant of low pass filter
        n_tilde.param("time_constant", t_const_, 0.3);
        // set root and tip
        n_tilde.param("root", root, std::string("torso_lift_link"));
        n_tilde.param("tip", tip, std::string("l_gripper_tool_frame"));

        // load robot model
        urdf::Model robot_model;
        robot_model.initParam("robot_description");

        KDL::Tree kdl_tree;
        kdl_parser::treeFromUrdfModel(robot_model, kdl_tree);
        kdl_tree.getChain(root, tip, chain_);
        jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));

        jnt_pos_.resize(chain_.getNrOfJoints());
        jacobian_.resize(chain_.getNrOfJoints());

        for (size_t i=0; i<chain_.getNrOfSegments(); i++) {
            if (chain_.getSegment(i).getJoint().getType() != KDL::Joint::None) {
                std::cerr << "kdl_chain(" << i << ") " << chain_.getSegment(i).getJoint().getName().c_str() << std::endl;
            }
        }
    }
bool kdl_urdf_tools::initialize_kinematics_from_urdf(
    const std::string &robot_description,
    const std::string &root_link,
    const std::string &tip_link,
    unsigned int &n_dof,
    KDL::Chain &kdl_chain,
    KDL::Tree &kdl_tree,
    urdf::Model &urdf_model)
{
  if(robot_description.length() == 0) {
    ROS_ERROR("URDF string is empty.");
    return false;
  }

  // Construct an URDF model from the xml string
  urdf_model.initString(robot_description);

  // Get a KDL tree from the robot URDF
  if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree)){
    ROS_ERROR("Failed to construct kdl tree");
    return false;
  }

  // Populate the KDL chain
  if(!kdl_tree.getChain(root_link, tip_link, kdl_chain))
  {
    ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
    ROS_ERROR_STREAM("  "<<root_link<<" --> "<<tip_link);
    ROS_ERROR_STREAM("  Tree has "<<kdl_tree.getNrOfJoints()<<" joints");
    ROS_ERROR_STREAM("  Tree has "<<kdl_tree.getNrOfSegments()<<" segments");
    ROS_ERROR_STREAM("  The segments are:");

    KDL::SegmentMap segment_map = kdl_tree.getSegments();
    KDL::SegmentMap::iterator it;

    for( it=segment_map.begin();
        it != segment_map.end();
        it++ )
    {
      ROS_ERROR_STREAM( "    "<<(*it).first);
    }

    return false;
  }

  // Store the number of degrees of freedom of the chain
  n_dof = kdl_chain.getNrOfJoints();

  return true;
}
bool PositionConstraint::initialize(const arm_navigation_msgs::PositionConstraint& constraint,
                const KDL::Chain& chain)
{
  constraint_ = constraint;
  link_id_ = -1;
  for (unsigned int i=0; i<chain.getNrOfSegments(); ++i)
  {
    if (constraint_.link_name == chain.getSegment(i).getName())
    {
      link_id_ = i;
      break;
    }
  }
  if (link_id_ == -1)
  {
    ROS_ERROR("CIK PositionConstraint: couldn't find link %s in chain.", constraint_.link_name.c_str());
    return false;
  }

  rosPointToKdlVector(constraint_.target_point_offset, offset_);
  rosPointToKdlVector(constraint_.position, desired_location_);

  if (constraint_.constraint_region_shape.type != arm_navigation_msgs::Shape::BOX ||
      constraint_.constraint_region_shape.dimensions.size() != 3)
  {
    ROS_ERROR("CIK PositionConstraint: we only handle 3-d BOX constraints currently.");
    return false;
  }

  KDL::Rotation constraint_rotation;
  rosQuaternionToKdlRotation(constraint_.constraint_region_orientation, constraint_rotation);
  constraint_rotation_inverse_ = constraint_rotation.Inverse();
  kdlRotationToEigenMatrix3d(constraint_rotation_inverse_, constraint_rotation_inverse_eigen_);

  return true;
}
Exemple #29
0
void controllerStateCallback(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg)
{
  unsigned int nj = chain.getNrOfJoints();
  
  JntArray q(7);
  std::cout << "Numbers of joints " << nj << "\n";
  for(int i = 0; i < 7; i++)
    {
      q(i) = msg->actual.positions[i];
    }
  std::cout << q(0) << " " << q(1) << " " << q(2) << " " << q(3) << " " << q(4) << " " << q(5) << " " << q(6)  << "\n";	
  Frame F_ist;
  fksolver1->JntToCart(q, F_ist);
  std::cout << F_ist <<"\n";
}
Exemple #30
0
void controllerStateCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
  unsigned int nj = chain.getNrOfJoints();
  std::vector<std::string> names = msg->name;
	std::vector<double> positions = msg->position;
	JntArray q = parseJointStates(names,positions);
 	std::cout << "Joints: " << q(0) << " " << q(1) << " " << q(2) << " " << q(3) << " " << q(4) << " " << q(5) << " " << q(6)  << "\n";	
  JntArray q_out(7);
 	Frame F_ist;
  fksolver1->JntToCart(q, F_ist);
  std::cout << F_ist <<"\n";
	int ret = iksolver1v->CartToJnt(q, getTwist(F_ist), q_out);
	if(ret >= 0)
	{
		sendVel(q, q_out);
  	std::cout << q_out(0) << " " << q_out(1) << " " << q_out(2) << " " << q_out(3) << " " << q_out(4) << " " << q_out(5) << " " << q_out(6)  << "\n";
	}	
	else
		std::cout << "Something went wrong" << "\n";	

}