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)); } }
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()); } }
// 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; } } } }
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()); } }
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); } }
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; }
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)); } }
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); }
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; }
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; }
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"; }
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; }
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"; }
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) {}
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(); }
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; }
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); } }
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; }
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; }
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; } }