コード例 #1
0
///reads manipulator joints
///@param Inputs the joint angles 
///@return The position of  the endfactor
void YouBotKDLInterface::getEndFactorPose(KDL::JntArray& iData, KDL::Frame& oPose)
{
    bool kinematic_status;
    KDL::ChainFkSolverPos_recursive fksolver = KDL::ChainFkSolverPos_recursive(chain) ;
    kinematic_status = fksolver.JntToCart(iData, oPose);

}
コード例 #2
0
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;
}
コード例 #3
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);
    }
}
コード例 #4
0
ファイル: run_cbf.cpp プロジェクト: ubi-agni/rviz_cbf_plugin
int main(int argc, char *argv[]) {
	std::string tip_frame;
	ros::init (argc, argv, "run_cbf");
	parse_arguments(argc, argv, tip_frame);

	ros::NodeHandle nh;

	rdf_loader::RDFLoader rdf("robot_description");
	boost::shared_ptr<srdf::Model> srdf = rdf.getSRDF();
	if (!srdf) srdf.reset(new srdf::Model());
	const boost::shared_ptr<urdf::ModelInterface>& urdf = rdf.getURDF();
	if (!urdf) {
		ROS_ERROR("couldn't load URDF model");
		return EXIT_FAILURE;
	}
	robot_model::RobotModelPtr robot_model(new robot_model::RobotModel(urdf, srdf));

	// fetch KDL tree
	KDL::Tree kdl_tree;
	if (!kdl_parser::treeFromUrdfModel(*urdf, kdl_tree)) {
		ROS_ERROR("Could not initialize KDL tree");
		return EXIT_FAILURE;
	}

	KDL::Chain kdl_chain;
	if (!kdl_tree.getChain(kdl_tree.getRootSegment()->first, tip_frame, kdl_chain)) {
		ROS_ERROR_STREAM("Could not find chain to " << tip_frame);
		return EXIT_FAILURE;
	}

	// create controller
	auto controller = createController(boost::make_shared<KDL::Chain>(kdl_chain));
	CBF::DummyResourcePtr  joints = boost::dynamic_pointer_cast<CBF::DummyResource>(controller->resource());
	CBF::DummyReferencePtr target = boost::dynamic_pointer_cast<CBF::DummyReference>(controller->reference());
	CBF::FloatVector target_vector(target->dim());

	// joint state publisher
	auto jsp = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
	// init joint_state message
	auto js_msg = init_message(kdl_chain);


	server.reset( new interactive_markers::InteractiveMarkerServer("cbf_control","",false) );
	// initialize marker with end-effector pose from forward kinematics
	KDL::ChainFkSolverPos_recursive fk = KDL::ChainFkSolverPos_recursive(kdl_chain);
	KDL::JntArray kdl_joints = KDL::JntArray(kdl_chain.getNrOfJoints());
	KDL::Frame kdl_pose;
	fk.JntToCart(kdl_joints, kdl_pose);

	tf::Pose tf_pose;
	tf::poseKDLToTF(kdl_pose, tf_pose);

	geometry_msgs::PoseStamped stamped;
	stamped.header.frame_id = kdl_tree.getRootSegment()->first;
	tf::poseTFToMsg(tf_pose, stamped.pose);

	bool bStalled = false;
	make6DofMarker(stamped, !bStalled);

	// set initial pose
	marker_feedback.pose = stamped.pose;

	// run controller
	ros::Rate rate(50); // 50 hz update rate
	while (ros::ok()) {
		// set target from marker feedback
		assign(target_vector.head(3), marker_feedback.pose.position);
		assign(target_vector.tail(3), marker_feedback.pose.orientation);
		target->set_reference(target_vector);

		// perform controller step
		unsigned int iMaxIter = 10;
		while (iMaxIter && controller->step() == false) {
			if (bStalled != controller->stalled()) {
				bStalled = controller->stalled();
				stamped.pose = marker_feedback.pose;
				make6DofMarker(stamped, !bStalled);
				if (bStalled) break;
			}
			--iMaxIter;
		}

		// fill + publish ros joint_state message
		update_message(js_msg, joints);
		jsp.publish(js_msg);

		// process ros messages
		ros::spinOnce();
		rate.sleep();
	}

	server.reset();

	return EXIT_SUCCESS;
}
コード例 #5
0
ファイル: sampleFkine.cpp プロジェクト: ipa-rmb/hermes
int main(int argc, char** argv){
  ros::init(argc, argv, "my_parser");
  if (argc != 2){
    ROS_ERROR("Need a urdf file as argument");
    return -1;
  }
  std::string urdf_file = argv[1];

  KDL::Tree hermes_tree;
  urdf::Model hermes_model;
  if (!hermes_model.initFile(urdf_file)){
    ROS_ERROR("Failed to parse urdf file");
    return -1;
  }
  if (!kdl_parser::treeFromUrdfModel(hermes_model,hermes_tree)){
	ROS_ERROR("Failed to construct kdl tree");
	return -1;
  }
  ROS_INFO("Successfully parsed urdf file to KDL");
 
  ROS_INFO("Try to solve forward kinematics for zero joint position ...");

  // Get Chain from the Tree
  bool exit_value;
  KDL::Chain hermes_chain;
  exit_value = hermes_tree.getChain("base","link7",hermes_chain);


  // Create solver based on kinematic chain
  KDL::ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(hermes_chain);

  // Create joint array
  KDL::JntArray q(hermes_chain.getNrOfJoints());
  
  // Assign values to the joints
  /*for(unsigned int i=0;i<hermes_chain.getNrOfJoints();i++){
        q(i)=0.0;
    }*/
 q(0) = -0.1;
 q(1) = 1.2;
 q(2) = -0.3;
 q(3) = 0.6;
 q(4) = 0.4;
 q(5) = 0.75;
 q(6) = -0.1;

  // Create the frame that will contain the results
    KDL::Frame cartpos;    
 
  // Calculate forward position kinematics
  bool kinematics_status;
  kinematics_status = fksolver.JntToCart(q,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 :(");
  }


  return 0;
}