示例#1
0
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));
    }


}
示例#2
0
void leatherman::printKDLChain(const KDL::Chain &c, std::string text)
{
  ROS_INFO("[%s] # segments: %d  # joints: %d", text.c_str(), c.getNrOfSegments(), c.getNrOfJoints());
  double r,p,y,jnt_pos = 0;
  for(size_t j = 0; j < c.getNrOfSegments(); ++j)
  {
    c.getSegment(j).pose(jnt_pos).M.GetRPY(r,p,y);
    ROS_INFO("[%s] [%2d] segment: %21s  xyz: %0.3f %0.3f %0.3f  rpy: %0.3f %0.3f %0.3f", 
        text.c_str(), int(j),
        c.getSegment(j).getName().c_str(),
        c.getSegment(j).pose(jnt_pos).p.x(),
        c.getSegment(j).pose(jnt_pos).p.y(),
        c.getSegment(j).pose(jnt_pos).p.z(),
        r,p,y);

    c.getSegment(j).getJoint().pose(jnt_pos).M.GetRPY(r,p,y);
    ROS_INFO("[%s]        joint: %21s  xyz: %0.3f %0.3f %0.3f  rpy: %0.3f %0.3f %0.3f  type: %s", 
        text.c_str(),
        c.getSegment(j).getJoint().getName().c_str(),
        c.getSegment(j).getJoint().pose(jnt_pos).p.x(),
        c.getSegment(j).getJoint().pose(jnt_pos).p.y(),
        c.getSegment(j).getJoint().pose(jnt_pos).p.z(),
        r,p,y,
        c.getSegment(j).getJoint().getTypeName().c_str());

  }
}
示例#3
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;
                }
            }
        }
    }
示例#4
0
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());
	}
}
示例#5
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);
  }
}
示例#6
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;
}
示例#8
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));
  }
}
示例#9
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);
}
示例#10
0
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());
}
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;
}
示例#12
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;
            }
        }
    }
示例#14
0
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;
}
示例#15
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";
}
示例#16
0
void FingerKinematics::initialise(KDL::Chain &chain,const std::string name){
    this->name = name;

    for(std::size_t i = 0; i < chain.segments.size();i++){
         joint_names.push_back(chain.segments[i].getJoint().getName());
    }

    ik_solver_vel_wdls = new KDL::ChainIkSolverVel_wdls(chain, 0.001, 5);
    ik_solver_vel_wdls->setLambda(0.01);
    Eigen::MatrixXd TS;  TS.resize(6,6);   TS.setIdentity();   TS(3,3) = 0;     TS(4,4) = 0;     TS(5,5) = 0;
    ik_solver_vel_wdls->setWeightTS(TS);

    fk_solver_pos        = new KDL::ChainFkSolverPos_recursive(chain);
    ik_solver_pos_NR     = new KDL::ChainIkSolverPos_NR(chain,*fk_solver_pos,*ik_solver_vel_wdls);

    num_joints = chain.getNrOfJoints();
    q.resize(num_joints);
    q.data.setZero();
    q_out = q;

}
示例#17
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";	

}
示例#18
0
void kdl_urdf_tools::joint_state_from_kdl_chain(
    const KDL::Chain &kdl_chain,
    sensor_msgs::JointState &joint_state)
{
  // Construct blank joint state message
  joint_state = sensor_msgs::JointState();

  // Add joint names
  for(std::vector<KDL::Segment>::const_iterator it=kdl_chain.segments.begin();
      it != kdl_chain.segments.end();
      it++)
  {
    joint_state.name.push_back(it->getJoint().getName());
  }

  // Get the #DOF
  unsigned int n_dof = kdl_chain.getNrOfJoints();

  // Resize joint vectors
  joint_state.position.resize(n_dof);
  joint_state.velocity.resize(n_dof);
  joint_state.effort.resize(n_dof);
}
int main()
{

    using namespace KDL;

    //Definition of kinematic chain
    KDL::Tree youBotTree;
    
    //note that it seems youbot_arm.urdf and youbot.urdf use different values.
    //parser's assumption is that it return joints which are not of type None.
    if (!kdl_parser::treeFromFile("/home/azamat/programming/ros-diamondback/youbot-ros-pkg/youbot_common/youbot_description/robots/youbot.urdf", youBotTree))
   {
        std::cout << "Failed to construct kdl tree" << std::endl;
        return false;
   }
   // else
   // {
   //      std::cout << "succeeded " << youBotTree.getNrOfJoints()<< std::endl;
   //      std::cout << "succeeded " << youBotTree.getNrOfSegments()<< std::endl;
   // }
 
    KDL::Chain chain;
    youBotTree.getChain("arm_link_0","arm_link_5",chain);
    unsigned int numberOfJoints = chain.getNrOfJoints();
    unsigned int numberOfLinks = chain.getNrOfSegments();
    // std::cout << "number of chain links " << numberOfLinks << std::endl;
    // std::cout << "number of chain joints " << numberOfJoints << std::endl;
    //~Definition of kinematic chain

    //Definition of constraints and external disturbances
    //--------------------------------------------------------------------------------------//
    //Constraint force matrix at the end-effector
    //What is the convention for the spatial force matrix; is it the same as in thesis?
    //NOTE AZAMAT: Constraint are also defined in local reference frame?!
    unsigned int numberOfConstraints = 2;
    Twist constraintForce;
    Twists constraintForces;
    for (unsigned int i = 0; i < numberOfJoints; i++)
    {   
        SetToZero(constraintForce);
        constraintForces.push_back(constraintForce);
    }

    constraintForces[0].vel[0] = 0;
    constraintForces[1].vel[1] = 0;
    constraintForces[2].vel[2] = 0;

    #ifdef X_CONSTRAINT_SET
        constraintForces[0].vel[0] = 1;
    #endif //~X_CONSTRAINT_SET

    #ifdef Y_CONSTRAINT_SET
        constraintForces[1].vel[1] = 1;
    #endif //~Y_CONSTRAINT_SET

    #ifdef Z_CONSTRAINT_SET
        constraintForces[2].vel[2] = 1;
    #endif //~Z_CONSTRAINT_SET

    //Acceleration energy and constraint matrix at  the end-effector
    Jacobian alpha(numberOfConstraints);
    JntArray betha(numberOfConstraints); //set to zero
    JntArray bethaControl(numberOfConstraints); 
    for (unsigned int i = 0; i < numberOfConstraints; i++)
    {
        alpha.setColumn(i, constraintForces[i]);
        betha(i) = 0.0;
        bethaControl(i) = 0.0;

    }
    //arm root acceleration
    Vector linearAcc(0.0, 0.0, 9.8); //gravitational acceleration along Z
    Vector angularAcc(0.0, 0.0, 0.0);
    Twist twist0(linearAcc, angularAcc);

    //external forces on the arm
    Wrench externalNetForce;
    Wrenches externalNetForces;
    for (unsigned int i = 0; i < numberOfLinks; i++)
    {
        externalNetForces.push_back(externalNetForce);
    }
    //~Definition of constraints and external disturbances


    //Definition of solver and initial configuration
    //-------------------------------------------------------------------------------------//

    ChainIdSolver_Vereshchagin constraintSolver(chain, twist0, numberOfConstraints);

    //These arrays of joint values contain actual and desired values
    //actual is generated by a solver and integrator
    //desired is given by an interpolator
    //error is the difference between desired-actual
    //0-actual, 1-desired, 2-error, 3-sum of error
    unsigned int k = 4;
    JntArray jointPoses[k];
    JntArray jointRates[k];
    JntArray jointAccelerations[k];
    JntArray jointTorques[k];
    JntArray jointControlTorques[k];
    for (unsigned int i = 0; i < k; i++)
    {
        JntArray jointValues(numberOfJoints);
        jointPoses[i] = jointValues;
        jointRates[i] = jointValues;
        jointAccelerations[i] = jointValues;
        jointTorques[i] = jointValues;
        jointControlTorques[i] = jointValues;
    }

    //cartesian space/link values
    //0-actual, 1-desire, 2-error, 3-errorsum
    Frames cartX[k];
    Twists cartXDot[k];
    Twists cartXDotDot[k];
    Twist accLink;
    KDL::Frame frameTemp;
    for (unsigned int i = 0; i < k; i++) //i is number of variables (actual, desired, error)
    {
        for (unsigned int j = 0; j < numberOfLinks; j++) //j is number of links
        {
            cartX[i].push_back(frameTemp);
            cartXDot[i].push_back(accLink);
            cartXDotDot[i].push_back(accLink);
        }

    }


    // Initial arm position configuration/constraint, negative in clockwise
    JntArray jointInitialPose(numberOfJoints);
    jointInitialPose(0) = -M_PI/4.0;
    jointInitialPose(1) = M_PI / 6.0;
    jointInitialPose(2) = M_PI / 12.0;
    jointInitialPose(3) = -M_PI / 3.0;
    jointInitialPose(4) = M_PI / 18.0;
    //corresponds to x=0.031827      y = -0.007827     z = 0.472972
    JntArray jointFinalPose(numberOfJoints);
    jointFinalPose(0) = M_PI / 2.0;
    jointFinalPose(1) = M_PI / 2.5;
    jointFinalPose(2) = 0.0;
    jointFinalPose(3) = 0.0;
    jointFinalPose(4) = 0.0;
    // corresponds to 0.024000      -0.367062     0.242885 

    for (unsigned int i = 0; i < numberOfJoints; i++)
    {
        //actual initial
        jointPoses[0](i) = jointInitialPose(i);
        //desired initial
        jointPoses[1](i) = jointInitialPose(i);
    }

    
    //test
    #ifdef FKPOSE_TEST
        ChainFkSolverPos_recursive fksolver(chain);
        Frame tempEE;
        fksolver.JntToCart(jointPoses[0], tempEE, 5);
        std::cout << tempEE << std::endl;
        fksolver.JntToCart(jointFinalPose, tempEE, 5);
        std::cout << tempEE << std::endl;
    #endif //~FKPOSE_TEST
    //~Definition of solver and initial configuration
    //-------------------------------------------------------------------------------------//


    //Definition of process main loop
    //-------------------------------------------------------------------------------------//
    double taskTimeConstant = 2; //Time required to complete the task move(frameinitialPose, framefinalPose) default T=10.0
    double simulationTime = 2.5 * taskTimeConstant;
    double timeDelta = 0.001;
    double timeToSettle = 0.5;
    int status;
    double alfa(0.0), beta(0.0), gamma(0.0);
    //Controller gain configurations
    
    double ksi[3] = {1.0, 1.0, 1.0}; //damping factor
    double Kp[3];
    Kp[0] = 240.1475/(timeToSettle*timeToSettle);//0.1475
    Kp[1] = 140.0/(timeToSettle*timeToSettle);
    Kp[2] = 240.0/(timeToSettle*timeToSettle);
    double Kv[3];
    Kv[0] = 55.2245*ksi[0]/timeToSettle;//2.2245
    Kv[1] = 30.6*ksi[1]/timeToSettle;
    Kv[2] = 30.6*ksi[1]/timeToSettle;
    double Ki[3] = {0.0, 0.0, 0.0};
    double K = 0.005;
    
    //For joint space control without constraints using computed torque
    // double ksi[2] = {1.0, 1.0}; //damping factor
    double Kpq[numberOfJoints];
    Kpq[0] = 2.5/(timeToSettle*timeToSettle); 
    Kpq[1] = 2.2/(timeToSettle*timeToSettle);
    Kpq[2] = 20.1/(timeToSettle*timeToSettle);
    Kpq[3] = 40.1/(timeToSettle*timeToSettle);
    Kpq[4] = 40.1/(timeToSettle*timeToSettle);
   
    double Kvq[numberOfJoints];
    Kvq[0] = .5*ksi[0]/timeToSettle;   
    Kvq[1] = 1.0*ksi[1]/timeToSettle;
    Kvq[2] = 3.0*ksi[1]/timeToSettle;
    Kvq[3] = 3.0*ksi[1]/timeToSettle;
    Kvq[4] = 3.0*ksi[1]/timeToSettle;
   
    //Interpolator parameters: 
    // Initial x=0.031827      y = -0.007827     z = 0.472972 
    // Final     0.024000          -0.367062         0.242885 
    //cartesian space
    double b0_x = 0.031827; //should come from initial joint configuration
    double b1_x = 0.0;
    double b2_x = ((0.031827 - b0_x)*3.0 / (simulationTime * simulationTime)); 
    double b3_x = -((0.031827 - b0_x)*2.0 / (simulationTime * simulationTime * simulationTime));

    double b0_y = -0.007827; //should come from initial joint configuration            
    double b1_y = 0.0;
    double b2_y =  ((-0.007827 - b0_y)*3.0 / (simulationTime * simulationTime)); 
    double b3_y = -((-0.007827 - b0_y)*2.0 / (simulationTime * simulationTime * simulationTime));

    double b0_z = 0.472972; //should come from initial joint configuration
    double b1_z = 0.0;
    double b2_z = ((0.242885 - b0_z)*3.0 / (simulationTime * simulationTime)); 
    double b3_z = -((0.242885 - b0_z)*2.0 / (simulationTime * simulationTime * simulationTime));


    //joint space
    double a0_q0 = jointInitialPose(0);
    double a1_q0 = 0.0;
    double a2_q0 = ((jointFinalPose(0)  - jointInitialPose(0))*3.0 / (simulationTime * simulationTime));
    double a3_q0 = -((jointFinalPose(0) - jointInitialPose(0))*2.0 / (simulationTime * simulationTime * simulationTime));

    double a0_q1 = jointInitialPose(1);
    double a1_q1 = 0.0;
    double a2_q1 = ((jointFinalPose(1) - jointInitialPose(1))*3.0 / (simulationTime * simulationTime));
    double a3_q1 = -((jointFinalPose(1) - jointInitialPose(1))*2.0 / (simulationTime * simulationTime * simulationTime));
    
    double a0_q2 = jointInitialPose(2);
    double a1_q2 = 0.0;
    double a2_q2 = ((jointFinalPose(2) - jointInitialPose(2))*3.0 / (simulationTime * simulationTime));
    double a3_q2 = -((jointFinalPose(2) - jointInitialPose(2))*2.0 / (simulationTime * simulationTime * simulationTime));

    double a0_q3 = jointInitialPose(3);
    double a1_q3 = 0.0;
    double a2_q3 = ((jointFinalPose(3) - jointInitialPose(3))*3.0 / (simulationTime * simulationTime));
    double a3_q3 = -((jointFinalPose(3) - jointInitialPose(3))*2.0 / (simulationTime * simulationTime * simulationTime));


    double a0_q4 = jointInitialPose(4);
    double a1_q4 = 0.0;
    double a2_q4 = ((jointFinalPose(4) - jointInitialPose(4))*3.0 / (simulationTime * simulationTime));
    double a3_q4 = -((jointFinalPose(4) - jointInitialPose(4))*2.0 / (simulationTime * simulationTime * simulationTime));
    
    double feedforwardJointTorque0 = 0.0;
    double feedforwardJointTorque1 = 0.0;
    double feedforwardJointTorque2 = 0.0;
    double feedforwardJointTorque3 = 0.0;
    double feedforwardJointTorque4 = 0.0;
    //---------------------------------------------------------------------------------------//


    //Main simulation loop
    for (double t = 0.0; t <= simulationTime; t = t + timeDelta)
    {
        
        //Interpolation (Desired) q = a0+a1t+a2t^2+a3t^3
       
        cartX[1][4].p[0] = b0_x + b1_x * t + b2_x * t * t + b3_x * t * t*t;
        cartX[1][4].p[1] = b0_y + b1_y * t + b2_y * t * t + b3_y * t * t*t;
        cartX[1][4].p[2] = b0_z + b1_z * t + b2_z * t * t + b3_z * t * t*t;
        cartXDot[1][4].vel[0] = b1_x + 2 * b2_x * t + 3 * b3_x * t*t;
        cartXDot[1][4].vel[1] = b1_y + 2 * b2_y * t + 3 * b3_y * t*t;
        cartXDot[1][4].vel[2] = b1_z + 2 * b2_z * t + 3 * b3_z * t*t;
        cartXDotDot[1][4].vel[0] = 2 * b2_x + 6 * b3_x*t;
        cartXDotDot[1][4].vel[1] = 2 * b2_y + 6 * b3_y*t;
        cartXDotDot[1][4].vel[2] = 2 * b2_z + 6 * b3_z*t;

        #ifdef DESIRED_CARTESIAN_VALUES
            printf("%f          %f      %f     %f         %f        %f      %f\n", t, cartX[1][4].p[0], cartX[1][4].p[1], cartX[1][4].p[2], cartXDot[1][4].vel[0], cartXDot[1][4].vel[1], cartXDot[1][4].vel[2]);
        #endif //~DESIRED_CARTESIAN_VALUES

        jointPoses[1](0) = a0_q0 + a1_q0 * t + a2_q0 * t * t + a3_q0 * t * t*t;
        jointPoses[1](1) = a0_q1 + a1_q1 * t + a2_q1 * t * t + a3_q1 * t * t*t;
        jointPoses[1](2) = a0_q2 + a1_q2 * t + a2_q2 * t * t + a3_q2 * t * t*t;
        jointPoses[1](3) = a0_q3 + a1_q3 * t + a2_q3 * t * t + a3_q3 * t * t*t;
        jointPoses[1](4) = a0_q4 + a1_q4 * t + a2_q4 * t * t + a3_q4 * t * t*t;  
        
        jointRates[1](0) = a1_q0 + 2 * a2_q0 * t + 3 * a3_q0 * t*t;
        jointRates[1](1) = a1_q1 + 2 * a2_q1 * t + 3 * a3_q1 * t*t;
        jointRates[1](2) = a1_q2 + 2 * a2_q2 * t + 3 * a3_q2 * t*t;
        jointRates[1](3) = a1_q3 + 2 * a2_q3 * t + 3 * a3_q3 * t*t;
        jointRates[1](4) = a1_q4 + 2 * a2_q4 * t + 3 * a3_q4 * t*t;
        
        jointAccelerations[1](0) = 2 * a2_q0 + 6 * a3_q0*t;
        jointAccelerations[1](1) = 2 * a2_q1 + 6 * a3_q1*t;
        jointAccelerations[1](2) = 2 * a2_q2 + 6 * a3_q2*t;
        jointAccelerations[1](3) = 2 * a2_q3 + 6 * a3_q3*t;
        jointAccelerations[1](4) = 2 * a2_q4 + 6 * a3_q4*t;

        #ifdef DESIRED_JOINT_VALUES
            printf("%f       %f     %f      %f     %f    %f     %f     %f    %f     %f      %f\n", t,jointPoses[1](0), jointPoses[1](1),jointPoses[1](2), jointPoses[1](3), jointPoses[1](4),
            jointRates[1](0), jointRates[1](1), jointRates[1](2), jointRates[1](3), jointRates[1](4));
        #endif //~DESIRED_JOINT_VALUES

        status = constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, bethaControl, externalNetForces, jointTorques[0], jointControlTorques[0]);
        constraintSolver.getLinkCartesianPose(cartX[0]);
        constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
        constraintSolver.getLinkCartesianAcceleration(cartXDotDot[0]);
        cartX[0][4].M.GetEulerZYX(alfa,beta,gamma);

        #ifdef ACTUAL_CARTESIAN_VALUES
            printf("%f          %f      %f     %f         %f        %f      %f\n",  t, cartX[0][4].p[0], cartX[0][4].p[1], cartX[0][4].p[2], alfa, beta, gamma);
        #endif //~ACTUAL_CARTESIAN_VALUES  

        //Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
        for (unsigned int i = 0; i < numberOfJoints; i++)
        {
            jointRates[0](i) = jointRates[0](i) + jointAccelerations[0](i) * timeDelta; //Euler Forward
            jointPoses[0](i) = jointPoses[0](i) + (jointRates[0](i) - jointAccelerations[0](i) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
        }
        
        #ifdef ACTUAL_JOINT_VALUES
            printf("%f       %f     %f      %f     %f    %f     %f     %f      %f     %f    %f     %f      %f    %f       %f      %f     %f    %f      %f     %f    %f\n", 
            t,jointPoses[0](0), jointPoses[0](1),jointPoses[0](2), jointPoses[0](3), jointPoses[0](4), jointRates[0](0), jointRates[0](1), jointRates[0](2), jointRates[0](3), jointRates[0](4),
            jointAccelerations[0](0), jointAccelerations[0](1), jointAccelerations[0](2), jointAccelerations[0](3), jointAccelerations[0](4), jointTorques[0](0), jointTorques[0](1), jointTorques[0](2), jointTorques[0](3), jointTorques[0](4));
        #endif //~ACTUAL_JOINT_VALUES

        //Error
        for (unsigned int i = 0; i < numberOfJoints; i++)
        {
            jointPoses[2](i) = jointPoses[1](i) - jointPoses[0](i);
            jointRates[2](i) = jointRates[1](i) - jointRates[0](i);
            jointAccelerations[2](i) = jointAccelerations[1](i) - jointAccelerations[0](i);
            // jointPoses[3](i) += timeDelta * jointPoses[2](i);
        }

        #ifdef JOINT_ERROR_VALUES
           printf("%f      %f     %f    %f     %f     %f\n", t,jointPoses[2](0), jointPoses[2](1),jointPoses[2](2), jointPoses[2](3), jointPoses[2](4));
        #endif //~JOINT_ERROR_VALUES

        cartX[2][4].p[0] = cartX[1][4].p[0] - cartX[0][4].p[0];
        cartX[2][4].p[1] = cartX[1][4].p[1] - cartX[0][4].p[1];
        cartX[2][4].p[2] = cartX[1][4].p[2] - cartX[0][4].p[2];
        cartXDot[2][4].vel[0] = cartXDot[1][4].vel[0] - cartXDot[0][4].vel[0];
        cartXDot[2][4].vel[1] = cartXDot[1][4].vel[1] - cartXDot[0][4].vel[1];
        cartXDot[2][4].vel[2] = cartXDot[1][4].vel[2] - cartXDot[0][4].vel[2];
        cartXDotDot[2][4].vel[0] = cartXDotDot[1][4].vel[0] - cartXDotDot[0][4].vel[0];
        cartXDotDot[2][4].vel[1] = cartXDotDot[1][4].vel[1] - cartXDotDot[0][4].vel[1];
        cartXDotDot[2][4].vel[2] = cartXDotDot[1][4].vel[2] - cartXDotDot[0][4].vel[2];
        cartX[3][4].p[0] += timeDelta * cartX[2][4].p[0]; //for integral term;
        cartX[3][4].p[1] += timeDelta * cartX[2][4].p[1];
        cartX[3][4].p[2] += timeDelta * cartX[2][4].p[2];
        
        #ifdef CARTESIAN_ERROR_VALUES
            printf("%f          %f      %f     %f         %f        %f      %f\n", t, cartX[2][4].p[0], cartX[2][4].p[1], cartX[2][4].p[2], cartXDot[2][4].vel[0], cartXDot[2][4].vel[1], cartXDot[2][4].vel[2]);
         #endif //~CARTESIAN_ERROR_VALUES

        //Regulator or controller  
        //acceleration energy control
        betha(0) = alpha(0, 0)*(K * cartXDotDot[2][numberOfLinks-1].vel[0] + (Kv[0]) * cartXDot[2][numberOfLinks-1].vel[0] + (Kp[0]) * cartX[2][numberOfLinks-1].p[0]+ Ki[1] * cartX[3][numberOfLinks-1].p[0]); 
        betha(1) = alpha(1, 1)*(K * cartXDotDot[2][numberOfLinks-1].vel[1] + (Kv[1]) * cartXDot[2][numberOfLinks-1].vel[1] + (Kp[1]) * cartX[2][numberOfLinks-1].p[1] + Ki[1] * cartX[3][numberOfLinks-1].p[1]);
        
        // priority posture control
        // jointControlTorques[0](0) = jointAccelerations[1](0) + (Kpq[0])*jointPoses[2](0) + (Kvq[0])*jointRates[2](0);
        // jointControlTorques[0](1) = jointAccelerations[1](1) + (Kpq[1])*jointPoses[2](1) + (Kvq[1])*jointRates[2](1);//computed joint torque control
        // jointControlTorques[0](2) = jointAccelerations[1](2) + (Kpq[2])*jointPoses[2](2) + (Kvq[2])*jointRates[2](2);
        // jointControlTorques[0](3) = jointAccelerations[1](3) + (Kpq[3])*jointPoses[2](3) + (Kvq[3])*jointRates[2](3);
        // jointControlTorques[0](4) = jointAccelerations[1](4) + (Kpq[3])*jointPoses[2](4) + (Kvq[4])*jointRates[2](4);
        
        //priority constraint control
        // feedforwardJointTorque0 = jointAccelerations[1](0) + (Kpq[0])*jointPoses[2](0) + (Kvq[0])*jointRates[2](0);
        feedforwardJointTorque1 = jointAccelerations[1](1) + (Kpq[1])*jointPoses[2](1) + (Kvq[1])*jointRates[2](1);
        feedforwardJointTorque2 = jointAccelerations[1](2) + (Kpq[2])*jointPoses[2](2) + (Kvq[2])*jointRates[2](2);
        feedforwardJointTorque3 = jointAccelerations[1](3) + (Kpq[3])*jointPoses[2](3) + (Kvq[3])*jointRates[2](3);
        feedforwardJointTorque4 = jointAccelerations[1](4) + (Kpq[4])*jointPoses[2](4) + (Kvq[4])*jointRates[2](4);
        // jointTorques[0](0) = jointTorques[0](0) + feedforwardJointTorque0;
        jointTorques[0](1) = jointTorques[0](1) + feedforwardJointTorque1;
        jointTorques[0](2) = jointTorques[0](2) + feedforwardJointTorque2;
        jointTorques[0](3) = jointTorques[0](3) + feedforwardJointTorque3;
        jointTorques[0](4) = jointTorques[0](4) + feedforwardJointTorque4;
  
        //For cartesian space control one needs to calculate from the obtained joint space value, new cartesian space poses.
        //Then based on the difference of the signal (desired-actual) we define a regulation function (controller)
        // this difference should be compensated either by joint torques.
        // externalNetForces[numberOfLinks-1].force[0] = ((Kv[0]) * cartXDot[2][numberOfLinks-1].vel[0] + (Kp[0]) * cartX[2][numberOfLinks-1].p[0] );
        // externalNetForces[numberOfLinks-1].force[1] = ((Kv[1]) * cartXDot[2][numberOfLinks-1].vel[1] + (Kp[1]) * cartX[2][numberOfLinks-1].p[1] ); 
        externalNetForces[numberOfLinks-1].force[2] = ((Kv[2]) * cartXDot[2][numberOfLinks-1].vel[2] + (Kp[2]) * cartX[2][numberOfLinks-1].p[2] ); 
  

    }
    //~Definition of process main loop
    //-------------------------------------------------------------------------------------//





    return 0;
}
    bool kuka_IK::cartPosInputHandle(RTT::base::PortInterface* portInterface){
    	input_cartPosPort.read(commandedPose);
#if DEBUG
    	cout << endl << "Pose.position.x  = " << commandedPose.position.x << endl;
    	cout << "Pose.position.y  = " << commandedPose.position.y << endl;
    	cout << "Pose.position.z  = " << commandedPose.position.z << endl;
#endif
    	// Read out the robot joint position
    	msr_jntPosPort.read(jntPos);


#if DEBUG
    	std::cout << " kuka-IK-component.cpp: jntPos" << std::endl;
   		for(int i = 0; i < 7; i++)  std::cout << jntPos[i] << " " ;
#endif
    	//Do IK and reset velocity profiles
    	commndedPoseJntPos = std::vector<double>(7,0.0);

#if DEBUG
    	cout <<  endl << endl;
    	cout << "Beginning of Super Debugging" << endl;
    	geometry_msgs::Pose tmpPose;
    	cartPosPort.read(tmpPose);
    	Frame tmpFrame;
    	tf::PoseMsgToKDL(tmpPose, tmpFrame);

    	cout << "Frame from Robot" << endl << tmpFrame << endl;

    	//Debugging Iterative IK
		KDL::Chain chain = Chain();
		chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Rotation::RotZ(M_PI), Vector(0.0, 0.0, 0.31))));
		chain.addSegment(Segment(Joint(Joint::RotY), Frame(Rotation::RotZ(M_PI), Vector(0.0, 0.0, 0.2))));
		chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.2))));
		chain.addSegment(Segment(Joint(Joint::RotY), Frame(Rotation::RotZ(M_PI), Vector(0.0, 0.0, 0.2))));
		chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.19))));
		chain.addSegment(Segment(Joint(Joint::RotY), Frame(Rotation::RotZ(M_PI))));
		chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(0.0, 0.0, 0.078))));

		ChainFkSolverPos_recursive fksolver(chain);

		// Create joint array
		unsigned int nj = chain.getNrOfJoints();
		KDL::JntArray jointpositions = JntArray(nj);

		// Assign some values to the joint positions
		std::cout << "jntPos: ";
		for(unsigned int i=0;i<nj;i++){
			jointpositions(i)=jntPos[i];
			std::cout << jntPos[i] << " ";
		}
		std::cout << std::endl;

		// Create the frame that will contain the results
		KDL::Frame cartpos;

		// Calculate forward position kinematics
		bool kinematics_status;
		kinematics_status = fksolver.JntToCart(jointpositions,cartpos);
		if(kinematics_status>=0){
			std::cout << cartpos <<std::endl;
			printf("%s \n","Success, thanks KDL!");
		}else{
			printf("%s \n","Error: could not calculate forward kinematics");
			std::cout << cartpos <<std::endl;
		}

		cout << "End of Super Debugging" << endl << endl;

		//End of Super Debugging
#endif

		//if (! KukaLWR_Kinematics::ikSolverAnalytical7DOF( commandedPose, commndedPoseJntPos) ){
    	//if (!(KukaLWR_Kinematics::ikSolver(jntPos, commandedPose, commndedPoseJntPos))){
    	if (!(KukaLWR_Kinematics::ikSolverIterative7DOF(jntPos, commandedPose, commndedPoseJntPos))){
    		cout << "lastCommandedPose cannot be achieved" << endl;
    		for(int i = 0; i < 7; i++)  std::cout << commndedPoseJntPos[i] << " " ;
    		std::cout << std::endl;
    	}
        
        sensor_msgs::JointState tmpJntState;
        tmpJntState.position.clear();
        for(int i=0; i < 7; i++){
    		tmpJntState.position.push_back(commndedPoseJntPos[i]);
    		tmpJntState.velocity.push_back(0.0);
    	}

    	output_jntPosPort.write(tmpJntState);
    	return true;

    }
ChainIkSolverPos_LMA::ChainIkSolverPos_LMA(
		const KDL::Chain& _chain,
		double _eps,
		int _maxiter,
		double _eps_joints
) :
	lastNrOfIter(0),
	lastSV(_chain.getNrOfJoints()>6?6:_chain.getNrOfJoints()),
	jac(6, _chain.getNrOfJoints()),
	grad(_chain.getNrOfJoints()),
	display_information(false),
	maxiter(_maxiter),
	eps(_eps),
	eps_joints(_eps_joints),
	chain(_chain),
	T_base_jointroot(_chain.getNrOfJoints()),
	T_base_jointtip(_chain.getNrOfJoints()),
	q(_chain.getNrOfJoints()),
	A(_chain.getNrOfJoints(), _chain.getNrOfJoints()),
	ldlt(_chain.getNrOfJoints()),
	svd(6, _chain.getNrOfJoints(),Eigen::ComputeThinU | Eigen::ComputeThinV),
	diffq(_chain.getNrOfJoints()),
	q_new(_chain.getNrOfJoints()),
    original_Aii(_chain.getNrOfJoints()),
    use_joint_limits(false)
{
	L(0)=1;
	L(1)=1;
	L(2)=1;
	L(3)=0.01;
	L(4)=0.01;
	L(5)=0.01;
}
ChainIkSolverPos_LMA::ChainIkSolverPos_LMA(
		const KDL::Chain& _chain,
		const Eigen::Matrix<double,6,1>& _L,
		double _eps,
		int _maxiter,
		double _eps_joints
) :
	lastNrOfIter(0),
	lastSV(_chain.getNrOfJoints()),
	jac(6, _chain.getNrOfJoints()),
	grad(_chain.getNrOfJoints()),
	display_information(false),
	maxiter(_maxiter),
	eps(_eps),
	eps_joints(_eps_joints),
	L(_L.cast<ScalarType>()),
	chain(_chain),
	T_base_jointroot(_chain.getNrOfJoints()),
	T_base_jointtip(_chain.getNrOfJoints()),
	q(_chain.getNrOfJoints()),
	A(_chain.getNrOfJoints(), _chain.getNrOfJoints()),
	tmp(_chain.getNrOfJoints()),
	ldlt(_chain.getNrOfJoints()),
	svd(6, _chain.getNrOfJoints(),Eigen::ComputeThinU | Eigen::ComputeThinV),
	diffq(_chain.getNrOfJoints()),
	q_new(_chain.getNrOfJoints()),
    original_Aii(_chain.getNrOfJoints()),
    use_joint_limits(false)
{}
示例#23
0
KukaKDL::KukaKDL(){
    segment_map.push_back("base");
    segment_map.push_back("00");
    segment_map.push_back("01");
    segment_map.push_back("02");
    segment_map.push_back("03");
    segment_map.push_back("04");
    segment_map.push_back("05");
    segment_map.push_back("06");
    segment_map.push_back("07");

    KDL::Chain chain;

    //joint 0
    chain.addSegment(KDL::Segment("base", KDL::Joint(KDL::Joint::None),
                KDL::Frame::DH_Craig1989(0.0, ALPHA0, R0, 0.0)
                ));

    //joint 1
	chain.addSegment(KDL::Segment("00", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0, ALPHA1, R1, 0.0),
                KDL::Frame::DH_Craig1989(0.0, ALPHA1, R1, 0.0).Inverse()*KDL::RigidBodyInertia(M1_KDL,
                    KDL::Vector(COGX1_KDL, COGY1_KDL, COGZ1_KDL),
                    KDL::RotationalInertia(XX1_KDL,YY1_KDL,ZZ1_KDL,XY1_KDL,XZ1_KDL,YZ1_KDL))));

    //joint 2 
    chain.addSegment(KDL::Segment("01", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA2, R2, 0.0),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA2, R2, 0.0).Inverse()*KDL::RigidBodyInertia(M2_KDL,
                    KDL::Vector(COGX2_KDL, COGY2_KDL, COGZ2_KDL),
                    KDL::RotationalInertia(XX2_KDL,YY2_KDL,ZZ2_KDL,XY2_KDL,XZ2_KDL,YZ2_KDL))));

    //joint 3
    chain.addSegment(KDL::Segment("02", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA3, R3, 0.0),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA3, R3, 0.0).Inverse()*KDL::RigidBodyInertia(M3_KDL,
                    KDL::Vector(COGX1_KDL, COGY1_KDL, COGZ1_KDL),
                    KDL::RotationalInertia(XX1_KDL,YY1_KDL,ZZ1_KDL,XY1_KDL,XZ1_KDL,YZ1_KDL))));

    //joint 4
    chain.addSegment(KDL::Segment("03", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0, ALPHA4, R4, 0.0),
                KDL::Frame::DH_Craig1989(0.0, ALPHA4, R4, 0.0).Inverse()*KDL::RigidBodyInertia(M4_KDL,
                    KDL::Vector(COGX4_KDL, COGY4_KDL, COGZ4_KDL),
                    KDL::RotationalInertia(XX4_KDL,YY4_KDL,ZZ4_KDL,XY4_KDL,XZ4_KDL,YZ4_KDL))));

    //joint 5
    chain.addSegment(KDL::Segment("04", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA5, R5, 0.0),
                KDL::Frame::DH_Craig1989(0.0,  ALPHA5, R5, 0.0).Inverse()*KDL::RigidBodyInertia(M5_KDL,
                    KDL::Vector(COGX5_KDL, COGY5_KDL, COGZ5_KDL),
                    KDL::RotationalInertia(XX5_KDL,YY5_KDL,ZZ5_KDL,XY5_KDL,XZ5_KDL,YZ5_KDL))));

    //joint 6
    chain.addSegment(KDL::Segment("05", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0, ALPHA6, R6, 0.0),
                KDL::Frame::DH_Craig1989(0.0, ALPHA6, R6, 0.0).Inverse()*KDL::RigidBodyInertia(M6_KDL,
                    KDL::Vector(COGX6_KDL, COGY6_KDL, COGZ6_KDL),
                    KDL::RotationalInertia(XX6_KDL,YY6_KDL,ZZ6_KDL,XY6_KDL,XZ6_KDL,YZ6_KDL))));
    //joint 7
    chain.addSegment(KDL::Segment("06", KDL::Joint(KDL::Joint::RotZ),
                KDL::Frame::DH_Craig1989(0.0, ALPHA7, R7, 0.0),
                KDL::Frame::DH_Craig1989(0.0, ALPHA7, R7, 0.0).Inverse()*KDL::RigidBodyInertia(M7_KDL,
                    KDL::Vector(COGX7_KDL, COGY7_KDL, COGZ7_KDL),
                    KDL::RotationalInertia(XX7_KDL,YY7_KDL,ZZ7_KDL,XY7_KDL,XZ7_KDL,YZ7_KDL))));

    //tool
    chain.addSegment(KDL::Segment("07", KDL::Joint(KDL::Joint::None), KDL::Frame::Identity()));
    

    tree.addChain(chain, "root");

    treejacsolver = new KDL::TreeJntToJacSolver(tree);
    fksolver = new KDL::TreeFkSolverPos_recursive(tree);
    fksolvervel = new KDL::ChainFkSolverVel_recursive(chain);

    q.resize(chain.getNrOfJoints());


	dynModelSolver = new KDL::ChainDynParam(chain,KDL::Vector(0.,0.,-9.81));
    qd.resize(chain.getNrOfJoints());
    externalWrenchTorque.resize(chain.getNrOfJoints());
    massMatrix.resize(chain.getNrOfJoints());
    corioCentriTorque.resize(chain.getNrOfJoints());
    gravityTorque.resize(chain.getNrOfJoints());
    frictionTorque.resize(chain.getNrOfJoints());

    actuatedDofs = Eigen::VectorXd::Constant(tree.getNrOfJoints(), 1);
    lowerLimits.resize(tree.getNrOfJoints());
    lowerLimits << -2.97, -2.10, -2.97, -2.10, -2.97, -2.10, -2.97;
    upperLimits.resize(tree.getNrOfJoints());
    upperLimits << 2.97, 2.10, 2.97, 2.10, 2.97, 2.10, 2.97;


    outdate();
}
示例#24
0
bool ik_solve(kinematics_msgs::GetPositionIK::Request  &req,
         kinematics_msgs::GetPositionIK::Response &res )
{
	unsigned int nj = chain.getNrOfJoints();
	
	JntArray q_min(nj);
	JntArray q_max(nj);
	for(int i = 0; i < nj; i+=2)
	{
		q_min(i) = -6.0;
		q_max(i) = 6.0;
	}
	for(int i = 1; i < nj; i+=2)
	{
		q_min(i) = -2.0;
		q_max(i) = 2.0;
	}

	
	ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
	ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
	ChainIkSolverPos_NR_JL iksolverpos(chain, q_min, q_max, fksolver1,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6

	JntArray q(nj);
	JntArray q_init(nj);
	for(int i = 0; i < nj; i++)
		q_init(i) = req.ik_request.ik_seed_state.joint_state.position[i];
	Frame F_dest;
	Frame F_ist;
	fksolver1.JntToCart(q_init, F_ist);
	tf::PoseMsgToKDL(req.ik_request.pose_stamped.pose, F_dest);
	std::cout << "Getting Goal\n";
	std::cout << F_dest;
	std::cout << "Calculated Position out of Configuration:\n";
	std::cout << F_ist <<"\n";

	int ret = iksolverpos.CartToJnt(q_init,F_dest,q);
	res.solution.joint_state.name = req.ik_request.ik_seed_state.joint_state.name;
	res.solution.joint_state.position.resize(nj);
	if(ret < 0)
	{
		res.error_code.val = -1;
		ROS_INFO("Inverse Kinematic found no solution");
		//std::cout << "RET: " << ret << std::endl;
		for(int i = 0; i < nj; i++)	
			res.solution.joint_state.position[i] = q_init(i);
	}
	else
	{
		ROS_INFO("Inverse Kinematic found a solution");
		res.error_code.val = 1;
		for(int i = 0; i < nj; i++)	
			res.solution.joint_state.position[i] = q(i);
	}
	//std::cout << "q_init\n";
	ROS_DEBUG("q_init: %f %f %f %f %f %f %f", q_init(0), q_init(1), q_init(2), q_init(3), q_init(4), q_init(5), q_init(6));
	ROS_DEBUG("q_out: %f %f %f %f %f %f %f", q(0), q(1), q(2), q(3), q(4), q(5), q(6));		
	//std::cout << "Solved with " << ret << " as return\n";
	//std::cout << q(0) << " " << q(1) << " " << q(2) << " " << q(3) << " " << q(4) << " " << q(5) << " " << q(6)  << "\n";	

	return true;
}
示例#25
0
int main()
{
    Vector Fend(3); Fend.zero();
    Vector Muend(Fend);

    Vector w0(3); Vector dw0(3); Vector ddp0(3);
    w0 = dw0=ddp0=0.0; ddp0[1]=g;

    int N = 7;
    
    Vector q(N);
    Vector dq(N);
    Vector ddq(N);
    
    
    Random rng;
    rng.seed(yarp::os::Time::now());
    for(int i=0;i<N-1;i++) 
    {
            q[i] = 0*CTRL_DEG2RAD*rng.uniform();
            dq[i] = 0*CTRL_DEG2RAD*rng.uniform();
            ddq[i] = 0*CTRL_DEG2RAD*rng.uniform();
    }
    
    
    for(int i=N-1;i<N;i++) 
    {
            q[i] = 0;
            dq[i] = 0;
            ddq[i] = 1;
    }
    
     
     
    
    
    L5PMDyn threeChain;
    
    
    iDynInvSensorL5PM threeChainSensor(threeChain.asChain(),"papare",DYNAMIC,iCub::skinDynLib::VERBOSE);
    int sensor_link = threeChainSensor.getSensorLink();
    
    q = threeChain.setAng(q);
    dq = threeChain.setDAng(dq);
    ddq = threeChain.setD2Ang(ddq);
    
    int nj=N;
    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];
    }
    
    threeChain.prepareNewtonEuler(DYNAMIC);
    threeChain.computeNewtonEuler(w0,dw0,ddp0,Fend,Muend);
    threeChainSensor.computeSensorForceMoment();


    // then we can retrieve our results...
    // forces moments and torques
    Matrix F = threeChain.getForces();
    Matrix Mu = threeChain.getMoments();
    Vector Tau = threeChain.getTorques();
    
    Matrix F_KDL = idynChainGetForces_usingKDL(threeChain,ddp0);
    Matrix Mu_KDL = idynChainGetMoments_usingKDL(threeChain,ddp0);
    Vector Tau_KDL = idynChainGetTorques_usingKDL(threeChain,ddp0);
    
    Matrix F_KDL_sens = idynChainGetForces_usingKDL(threeChain,threeChainSensor,ddp0);
    Matrix Mu_KDL_sens = idynChainGetMoments_usingKDL(threeChain,threeChainSensor,ddp0);
    Vector Tau_KDL_sens = idynChainGetTorques_usingKDL(threeChain,threeChainSensor,ddp0);
    
    Matrix p_idyn(3,N),p_kdl_no_sens(3,N),p_kdl_sens(3,N+1);
    
    for(int l=0;l<N;l++) 
    {
        p_idyn.setCol(l,threeChain.getH(l).subcol(0,3,3));
    }
    KDL::Chain threeChainKDL;
    
    std::vector<std::string> dummy;
    std::string dummy_str = "";
    
    for(int l=0;l<N;l++) {
        Vector p_kdl;
        idynChain2kdlChain(*(threeChain.asChain()),threeChainKDL,dummy,dummy,dummy_str,dummy_str,l+1);
        KDL::ChainFkSolverPos_recursive posSolver = KDL::ChainFkSolverPos_recursive(threeChainKDL);
        KDL::Frame cartpos;
        KDL::JntArray jpos;
        jpos.resize(l+1);
        for(int p=0;p<jpos.rows();p++) jpos(p) = jointpositions(p);
        posSolver.JntToCart(jpos,cartpos);
        to_iDyn(cartpos.p,p_kdl);
        p_kdl_no_sens.setCol(l,p_kdl);
    }
        KDL::Chain threeChainKDLsens;
    for(int l=0;l<N+1;l++) {
        Vector p_kdl;
        idynSensorChain2kdlChain(*(threeChain.asChain()),threeChainSensor,threeChainKDLsens,dummy,dummy,dummy_str,dummy_str,l+1);
        KDL::ChainFkSolverPos_recursive posSolver = KDL::ChainFkSolverPos_recursive(threeChainKDLsens);
        KDL::Frame cartpos;
        KDL::JntArray jpos;
        if( l <= sensor_link ) { 
            jpos.resize(l+1);
            for(int p=0;p<jpos.rows();p++) jpos(p) = jointpositions(p);
        } 
        //if( l >= sensor_link ) 
        else {
            jpos.resize(l);
            for(int p=0;p<jpos.rows();p++) jpos(p) = jointpositions(p);
        }
        cout << "l " << l << " nrJoints " << threeChainKDLsens.getNrOfJoints() <<  " nrsegments " << threeChainKDLsens.getNrOfSegments() <<" jpos dim " << jpos.rows() << endl;
        assert(posSolver.JntToCart(jpos,cartpos) >= 0);
        to_iDyn(cartpos.p,p_kdl);
        p_kdl_sens.setCol(l,p_kdl);
    }
    printMatrix("p_idyn",p_idyn);
    printMatrix("p_kdl_no_sens",p_kdl_no_sens);
    printMatrix("p_kdl_sens",p_kdl_sens);
    
    
    
    cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~`KDL Chain~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~``" << endl << threeChainKDL << endl;
    cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~KDL Chain sens~~~~~~~~~~~~~~~~~~~~~~~~~~~~`" << endl << threeChainKDLsens << endl;
    

    //printKDLchain("threeChainKDLsens",threeChainKDLsens);

    printMatrix("F",F);
    printMatrix("F_KDL",F_KDL);
    printMatrix("F_KDL_sens",F_KDL_sens);
    printMatrix("Mu",Mu);
    printMatrix("Mu_KDL",Mu_KDL);
    printMatrix("Mu_KDL_sens",Mu_KDL_sens);
    printVector("Tau",Tau);
    printVector("Tau_KDL",Tau_KDL);
    printVector("Tau_KDL_sens",Tau_KDL_sens);
    
    for(int l=0;l<F_KDL.cols();l++)
    {
            YARP_ASSERT(EQUALISH(F_KDL.getCol(l),F.getCol(l)));
            YARP_ASSERT(EQUALISH(F_KDL_sens.getCol(l),F.getCol(l)));
            YARP_ASSERT(EQUALISH(Mu_KDL.getCol(l),Mu.getCol(l)));
            YARP_ASSERT(EQUALISH(Mu_KDL_sens.getCol(l),Mu.getCol(l)));
    }
    
    for(int l=0;l<Tau.size();l++) {
        YARP_ASSERT(abs(Tau(l)-Tau_KDL(l))<delta);
        YARP_ASSERT(abs(Tau(l)-Tau_KDL_sens(l))<delta);
    }
}
示例#26
0
int main(int argc, char **argv)
{
	ros::init(argc, argv, "leg_kinematics_pub");
	ros::NodeHandle node;
	KDL::Tree tree;
	KDL::Chain chain;
	KDL::ChainFkSolverPos_recursive* fk_solver;
	KDL::HP_ChainIkSolverPos_NR_JL* ik_solver_pos;
	KDL::ChainIkSolverVel_pinv* ik_solver_vel;

	ros::Publisher leg_msg_pub = node.advertise<crab_msgs::LegJointsState>("leg_states", 1);
	ros::Publisher joint_msg_pub = node.advertise<sensor_msgs::JointState>("leg_joints_states", 1);
	ros::Rate loop_rate(30);

	std::string robot_desc_string;
	node.param("robot_description", robot_desc_string, std::string("robot_description"));
	if (!kdl_parser::treeFromString(robot_desc_string, tree)){
		ROS_ERROR("Failed to construct kdl tree");
		return false;
		}
	if (!tree.getChain("base_link", "tibia_foot_r1", chain)) {
		ROS_ERROR("Failed to construct kdl chain");
		return false;
	   }
	ROS_INFO("Construct kdl chain");

	fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
	ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
//	Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);
//	matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;
//	ik_solver_vel -> setWeightTS(matrix_Mx);
	KDL::JntArray joint_min(chain.getNrOfJoints());
	KDL::JntArray joint_max(chain.getNrOfJoints());
	joint_min(0) = -1.57;	joint_min(1) = -1.57;	joint_min(2) = -1.57;
	joint_max (0) = 1.57;	joint_max (1) = 1.57;	joint_max (2) = 1.57;
	ik_solver_pos = new KDL::HP_ChainIkSolverPos_NR_JL (chain, joint_min, joint_max,
															*fk_solver, *ik_solver_vel, 100, 0.0001);

	KDL::JntArray q_init(chain.getNrOfJoints());
	q_init (0) = 0;	q_init (1) = 0;	q_init (2) = 0;
	KDL::JntArray q_out(chain.getNrOfJoints());

	crab_msgs::LegJointsState leg_msg;
	sensor_msgs::JointState joint_msg;
	int ik_valid;
	double x_0 = 0.14208, y_0 = -0.14555, z_0 = -0.11245, x, y, fi;  //0.14208; -0.14555; -0.11145
	while (ros::ok()){
		if (fi > 6.28) fi = 0;
		x = x_0 + 0.05 * cos(fi);
		y = y_0 + 0.05 * sin(fi);
		KDL::Frame p_in (KDL::Vector(x, y, z_0));

		ik_valid = ik_solver_pos -> CartToJnt(q_init, p_in, q_out);
		if (ik_valid >= 0){
			ROS_INFO("\nJoint 1: %f\nJoint 2: %f\nJoint 3: %f\n", q_out(0),q_out(1),q_out(2));
			for (int i=0; i<3; i++){
				leg_msg.joint[i] = q_out(i);
			}
			leg_msg_pub.publish(leg_msg);
			joint_msg.name.push_back("coxa_joint_r1");
			joint_msg.position.push_back(q_out(0));
			joint_msg.name.push_back("femur_joint_r1");
			joint_msg.position.push_back(q_out(1));
			joint_msg.name.push_back("tibia_joint_r1");
			joint_msg.position.push_back(q_out(2));
			joint_msg_pub.publish(joint_msg);
			joint_msg.name.clear();
			joint_msg.position.clear();
		}
		else {
			ROS_ERROR("IK not found");
		}
		fi += 0.05;
		q_init = q_out;
		ros::spinOnce();
		loop_rate.sleep();
	}
	return 0;
}
示例#27
0
bool ik_solve(kinematics_msgs::GetPositionIK::Request  &req,
         kinematics_msgs::GetPositionIK::Response &res )
{
	ROS_INFO("get_ik_service has been called!");
	
	if(req.ik_request.ik_link_name.length() == 0)
		my_tree.getChain("base_link","arm_7_link", chain);
	else
		my_tree.getChain("base_link",req.ik_request.ik_link_name, chain);
	
	
	
	unsigned int nj = chain.getNrOfJoints();
	
	JntArray q_min(nj);
	JntArray q_max(nj);
	
	//ToDo: get joint limits from robot_description on parameter_server or use /cob_arm_kinematics/get_ik_solver_info!!!
	for(int i = 0; i < nj; i+=2)
	{
		q_min(i) =-2.9670;		//adjusted due to cob_description/lbr.urdf.xacro
		q_max(i) = 2.9670;
	}
	for(int i = 1; i < nj; i+=2)
	{
		q_min(i) =-2.0943951;	//adjusted due to cob_description/lbr.urdf.xacro
		q_max(i) = 2.0943951;
	}
	
	ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
	ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
	ChainIkSolverPos_NR_JL iksolverpos(chain, q_min, q_max, fksolver1,iksolver1v,1000,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6

	JntArray q(nj);
	JntArray q_init(nj);
	for(int i = 0; i < nj; i++)
		q_init(i) = req.ik_request.ik_seed_state.joint_state.position[i];
	Frame F_dest;
	Frame F_ist;
	fksolver1.JntToCart(q_init, F_ist);
	tf::PoseMsgToKDL(req.ik_request.pose_stamped.pose, F_dest);
	std::cout << "Getting Goal\n";
	std::cout << F_dest <<"\n";
	std::cout << "Calculated Position out of Seed-Configuration:\n";
	std::cout << F_ist <<"\n";

	//uhr-fm: here comes the actual IK-solver-call -> could be replaced by analytical-IK-solver (cob)
	int ret = iksolverpos.CartToJnt(q_init,F_dest,q);
	
	//res.solution.joint_state.name = req.ik_request.ik_seed_state.joint_state.name;	
	res.solution.joint_state.name.resize(nj);
	res.solution.joint_state.name[0]="arm_1_joint";
	res.solution.joint_state.name[1]="arm_2_joint";
	res.solution.joint_state.name[2]="arm_3_joint";
	res.solution.joint_state.name[3]="arm_4_joint";
	res.solution.joint_state.name[4]="arm_5_joint";
	res.solution.joint_state.name[5]="arm_6_joint";
	res.solution.joint_state.name[6]="arm_7_joint";
	
	res.solution.joint_state.position.resize(nj);
	res.solution.joint_state.velocity.resize(nj);
	res.solution.joint_state.effort.resize(nj);
	if(ret < 0)
	{
		//res.error_code.val = 0;
		res.error_code.val = res.error_code.NO_IK_SOLUTION;
		ROS_INFO("Inverse Kinematic found no solution");
		std::cout << "RET: " << ret << std::endl;
		for(int i = 0; i < nj; i++)
		{
			res.solution.joint_state.position[i] = q_init(i);
			res.solution.joint_state.velocity[i] = 0.0;
			res.solution.joint_state.effort[i] = 0.0;
		}
	}
	else
	{
		ROS_INFO("Inverse Kinematic found a solution");
		//res.error_code.val = 1;
		res.error_code.val = res.error_code.SUCCESS;
		for(int i = 0; i < nj; i++)
		{
			res.solution.joint_state.position[i] = q(i);
			res.solution.joint_state.velocity[i] = 0.0;
			res.solution.joint_state.effort[i] = 0.0;
		}
	}
	//std::cout << "q_init\n";
	ROS_INFO("q_init: %f %f %f %f %f %f %f", q_init(0), q_init(1), q_init(2), q_init(3), q_init(4), q_init(5), q_init(6));
	ROS_INFO("q_out: %f %f %f %f %f %f %f", q(0), q(1), q(2), q(3), q(4), q(5), q(6));		
	//std::cout << "Solved with " << ret << " as return\n";
	//std::cout << q(0) << " " << q(1) << " " << q(2) << " " << q(3) << " " << q(4) << " " << q(5) << " " << q(6)  << "\n";	

	return true;
}
示例#28
0
int main(int argc, char **argv)
{
    ros::init(argc, argv, "leg_kinematics");
    KDL::Tree tree;
    KDL::Chain chain;
    ros::NodeHandle node;
    KDL::ChainFkSolverPos_recursive* fk_solver;
    KDL::HP_ChainIkSolverPos_NR_JL *ik_solver_pos;
    KDL::ChainIkSolverVel_pinv* ik_solver_vel;

    std::string robot_desc_string;
    node.param("robot_description", robot_desc_string, std::string("robot_description"));
    if (!kdl_parser::treeFromString(robot_desc_string, tree)) {
        ROS_ERROR("Failed to construct kdl tree");
        return false;
    }

    if (!tree.getChain("base_link", "tibia_foot_r1", chain)) {
        ROS_ERROR("Failed to construct kdl chain");
        return false;
    }
    ROS_INFO("Construct kdl chain");

//		unsigned int nj = chain.getNrOfJoints();
//		unsigned int js = chain.getNrOfSegments();
//		std_msgs::String msg;
//		std::stringstream ss;
//		ss << "# J: " << nj << "  # S: " << js;
//		msg.data = ss.str();
//		ROS_INFO("Construct kdl tree");
//		ROS_INFO("%s", msg.data.c_str()) ;

    fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
    ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
//	Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);
//	matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;
//	ik_solver_vel -> setWeightTS(matrix_Mx);
    KDL::JntArray joint_min(chain.getNrOfJoints());
    KDL::JntArray joint_max(chain.getNrOfJoints());
    joint_min(0) = -1.57;
    joint_min(1) = -1.57;
    joint_min(2) = -1.57;
    joint_max (0) = 1.57;
    joint_max (1) = 1.57;
    joint_max (2) = 1.57;
    ik_solver_pos = new KDL::HP_ChainIkSolverPos_NR_JL (chain, joint_min, joint_max,
            *fk_solver, *ik_solver_vel, 20, 0.0001);

    KDL::JntArray q_init(chain.getNrOfJoints());
    q_init (0) = 0;
    q_init (1) = 0;
    q_init (2) = 0;
    KDL::JntArray q_out(chain.getNrOfJoints());

//	KDL::Segment my_seg = chain.getSegment(3);
//	KDL::Frame my_fr;
//
//	if (fk_solver.JntToCart(q_init, my_fr) >= 0){
//
//			std_msgs::String msg;
//			std::stringstream ss;
//			ss << "# 0: " << my_fr.p[0] << "  # 1: " << my_fr.p[1] << "  # 2: " << my_fr.p[2];
//			msg.data = ss.str();
//			ROS_INFO("Construct kdl tree");
//			ROS_INFO("%s", msg.data.c_str()) ;
//		}

    double x = 0.16509, y = -0.18567, z = 0.053603;  //0.16509; -0.18567; 0.053603
    KDL::Frame p_in (KDL::Vector(0.12291, -0.085841, -0.16766));

    int ik_valid = ik_solver_pos -> CartToJnt(q_init, p_in, q_out);
    if (ik_valid >= 0) {
        ROS_INFO("\nJoint 1: %f\nJoint 2: %f\nJoint 3: %f\n", q_out(0),q_out(1),q_out(2));
    }
    else {
        ROS_ERROR("IK not found");
    }

    return 0;
}
    void callbackJointState(const JointStateConstPtr& state)
    {
        std::map<std::string, double> joint_name_position;
        if (state->name.size() != state->position.size()) {
            ROS_ERROR("Robot state publisher received an invalid joint state vector");
            return;
        }

        // determine least recently published joint
        ros::Time last_published = ros::Time::now();
        for (unsigned int i=0; i<state->name.size(); i++) {
            ros::Time t = last_publish_time_[state->name[i]];
            last_published = (t < last_published) ? t : last_published;
        }

        if (state->header.stamp >= last_published + publish_interval_) {
            // get joint positions from state message
            std::map<std::string, double> joint_positions;
            std::map<std::string, double> joint_efforts;
            for (unsigned int i=0; i<state->name.size(); i++) {
                joint_positions.insert(make_pair(state->name[i], state->position[i]));
                joint_efforts.insert(make_pair(state->name[i], state->effort[i]));
            }

            KDL::JntArray  tau;
            KDL::Wrench    F;
            Eigen::Matrix<double,Eigen::Dynamic,6> jac_t;
            Eigen::Matrix<double,6,Eigen::Dynamic> jac_t_pseudo_inv;
            tf::StampedTransform transform;
            KDL::Wrench    F_pub;
            tf::Vector3 tf_force;
            tf::Vector3 tf_torque;

            tau.resize(chain_.getNrOfJoints());

            //getPositions;
            for (size_t i=0, j=0; i<chain_.getNrOfSegments(); i++) {
                if (chain_.getSegment(i).getJoint().getType() == KDL::Joint::None)
                    continue;

                // check
                std::string joint_name = chain_.getSegment(i).getJoint().getName();
                if ( joint_positions.find(joint_name) == joint_positions.end() ) {
                    ROS_ERROR("Joint '%s' is not found in joint state vector", joint_name.c_str());
                }

                // set position
                jnt_pos_(j) = joint_positions[joint_name];
                tau(j) = joint_efforts[joint_name];
                j++;
            }

            jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
            jac_t = jacobian_.data.transpose();
            if ( jacobian_.columns() >= jacobian_.rows() ) {
                jac_t_pseudo_inv =(jac_t.transpose() * jac_t).inverse() *  jac_t.transpose();
            } else {
                jac_t_pseudo_inv =jac_t.transpose() * ( jac_t *  jac_t.transpose() ).inverse();
            }
#if 1
            {
                ROS_INFO("jac_t# jac_t : ");
                Eigen::Matrix<double,6,6> mat_i =  mat_i = jac_t_pseudo_inv * jac_t;
                for (unsigned int i = 0; i < 6; i++) {
                    std::stringstream ss;
                    for (unsigned int j=0; j<6; j++)
                        ss << std::fixed << std::setw(8) << std::setprecision(4) << mat_i(j,i) << " ";
                    ROS_INFO_STREAM(ss.str());
                }
            }
#endif
            // f = - inv(jt) * effort
            for (unsigned int j=0; j<6; j++)
            {
                F(j) = 0;
                for (unsigned int i = 0; i < jnt_pos_.rows(); i++)
                {
                    F(j) += -1 * jac_t_pseudo_inv(j, i) * tau(i);
                }
            }

            //low pass filter
            F += (F_pre_ - F) * exp(-1.0 / t_const_);
            for (unsigned int j=0; j<6; j++) {
                F_pre_(j) = 0;
            }
            F_pre_ += F;

            //tf transformation
            tf::vectorKDLToTF(F.force, tf_force);
            tf::vectorKDLToTF(F.torque, tf_torque);
            try {
                listener_.waitForTransform( tip, root, state->header.stamp, ros::Duration(1.0));
                listener_.lookupTransform( tip, root, state->header.stamp , transform);
            }
            catch (tf::TransformException ex) {
                ROS_ERROR("%s",ex.what());
                ros::Duration(1.0).sleep();
            }
            for (unsigned int j=0; j<3; j++) {
                F_pub.force[j] = transform.getBasis()[j].dot(tf_force);
                F_pub.torque[j] = transform.getBasis()[j].dot(tf_torque);
            }

            geometry_msgs::WrenchStamped msg;
            msg.header.stamp = state->header.stamp;
            msg.header.frame_id = tip;
            // http://code.ros.org/lurker/message/20110107.151127.7177af06.nl.html
            //tf::WrenchKDLToMsg(F,msg.wrench);
            msg.wrench.force.x = F_pub.force[0];
            msg.wrench.force.y = F_pub.force[1];
            msg.wrench.force.z = F_pub.force[2];
            msg.wrench.torque.x = F_pub.torque[0];
            msg.wrench.torque.y = F_pub.torque[1];
            msg.wrench.torque.z = F_pub.torque[2];
            wrench_stamped_pub_.publish(msg);

            {
                ROS_INFO("jacobian : ");
                for (unsigned int i = 0; i < jnt_pos_.rows(); i++) {
                    std::stringstream ss;
                    for (unsigned int j=0; j<6; j++)
                        ss << std::fixed << std::setw(8) << std::setprecision(4) << jacobian_(j,i) << " ";
                    ROS_INFO_STREAM(ss.str());
                }
                ROS_INFO("effort : ");
                std::stringstream sstau;
                for (unsigned int i = 0; i < tau.rows(); i++) {
                    sstau << std::fixed << std::setw(8) << std::setprecision(4) << tau(i) << " ";
                }
                ROS_INFO_STREAM(sstau.str());
                ROS_INFO("force : ");
                std::stringstream ssf;
                for (unsigned int j = 0; j < 6; j++) {
                    ssf << std::fixed << std::setw(8) << std::setprecision(4) << F(j) << " ";
                }
                ROS_INFO_STREAM(ssf.str());
            }

            // store publish time in joint map
            for (unsigned int i=0; i<state->name.size(); i++)
                last_publish_time_[state->name[i]] = state->header.stamp;
        }
    }