void convertKDLJacToVispMat(const KDL::Jacobian &kdlJac, vpMatrix &vispMat) { for(unsigned int i=0; i<kdlJac.rows(); ++i) { for(unsigned int j=0; j<kdlJac.columns(); ++j) { vispMat[i][j] = kdlJac.data(i,j); } } }
vpMatrix kdlJacobianToVispJacobian(const KDL::Jacobian& kdlJacobian, const vpHomogeneousMatrix& bMe) { vpMatrix vpJacobian(kdlJacobian.rows(), kdlJacobian.columns()); KDL::Jacobian kdlJacobianCopy = kdlJacobian; // Fix reference point so that it is in the base frame vpTranslationVector bOe; //origin of end frame in base frame bMe.extract(bOe); kdlJacobianCopy.changeRefPoint(KDL::Vector(-bOe[0], -bOe[1], -bOe[2])); pal::convertKDLJacToVispMat(kdlJacobianCopy, vpJacobian);// KDL data --> Visp data return vpJacobian; }
// ------------------------------------------------------------------------------------- KDL::Jacobian JacoIKSolver::desiredJacobian(Eigen::MatrixXd qd) { KDL::JntArray joint_in; KDL::Jacobian jac; joint_in.resize(6); jac.resize(chain_.getNrOfJoints()); qd = DTR*qd; for (int i = 0; i < 6; i++){ joint_in(i) = qd(0,i); } jnt_to_jac_solver_->JntToJac(joint_in, jac); return jac; }
KDL::Jacobian JacoIKSolver::jointToJacobian(JacoAngles JAngle) { KDL::JntArray joint_in; KDL::Jacobian jac; joint_in.resize(6); jac.resize(chain_.getNrOfJoints()); joint_in(0) = ( JAngle.Actuator1 - 180.0 ) * DTR; joint_in(1) = ( JAngle.Actuator2 - 270.0 ) * DTR; joint_in(2) = ( JAngle.Actuator3 - 90.0 ) * DTR; joint_in(3) = ( JAngle.Actuator4 - 180.0 ) * DTR; joint_in(4) = ( JAngle.Actuator5 - 180.0 ) * DTR; joint_in(5) = ( JAngle.Actuator6 - 270.0 ) * DTR; jnt_to_jac_solver_->JntToJac(joint_in, jac); return jac; }
void ITRCartesianImpedanceController::multiplyJacobian(const KDL::Jacobian& jac, const KDL::Wrench& src, KDL::JntArray& dest) { Eigen::Matrix<double,6,1> w; w(0) = src.force(0); w(1) = src.force(1); w(2) = src.force(2); w(3) = src.torque(0); w(4) = src.torque(1); w(5) = src.torque(2); Eigen::MatrixXd j(jac.rows(), jac.columns()); j = jac.data; j.transposeInPlace(); Eigen::VectorXd t(jac.columns()); t = j*w; dest.resize(jac.columns()); for (unsigned i=0; i<jac.columns(); i++) dest(i) = t(i); }
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; } } }
void CartTrajInterp::updateHook(){ // Read the ports RTT::FlowStatus ftraj = this->port_traj_in_.read(this->traj_in_); RTT::FlowStatus fp = this->port_joint_position_in_.read(this->joint_position_in_); RTT::FlowStatus fv = this->port_joint_velocity_in_.read(this->joint_velocity_in_); // If we get a new trajectory update it and start with point 0 if(ftraj == RTT::NewData) { RTT::log(RTT::Info) << "New trajectory with "<<traj_in_.result.planned_trajectory.joint_trajectory.points.size() <<" points" << RTT::endlog(); if(!this->interpolate(traj_in_.result.planned_trajectory.joint_trajectory, traj_curr_)){ RTT::log(RTT::Error) << "Interpolation of the trajectory failed" << RTT::endlog(); return; } RTT::log(RTT::Info) << "Interpolated traj with "<<traj_curr_.points.size() <<" points" << RTT::endlog(); this->traj_pt_nb_ = 0; } // Return if not giving any robot update (might happend during startup) if(fp == RTT::NoData || fv == RTT::NoData) return // Update the chain model this->arm_.setState(this->joint_position_in_,this->joint_velocity_in_); this->arm_.updateModel(); // Check trajectory is not empty if (this->traj_curr_.points.size() < 1) return; // Check trajectory is not finished if (this->traj_pt_nb_ > this->traj_curr_.points.size() - 1) return; // Pick current point in trajectory for(unsigned int i = 0; i < this->dof_; ++i){ this->pt_in_.q(i) = this->traj_curr_.points[this->traj_pt_nb_].positions[i]; this->pt_in_.qdot(i) = this->traj_curr_.points[this->traj_pt_nb_].velocities[i] ; this->pt_in_.qdotdot(i) = this->traj_curr_.points[this->traj_pt_nb_].accelerations[i] ; } // Compute FK KDL::FrameVel traj_pt_vel; KDL::JntArrayVel pt_in_vel; pt_in_vel.q = this->pt_in_.q; pt_in_vel.qdot = this->pt_in_.qdot; KDL::Twist JdotQdot; // If FK or JdotQdot solver fails start over if ((this->fk_solver_vel_->JntToCart(pt_in_vel, traj_pt_vel) == 0) && (this->jntToJacDotSolver_->JntToJacDot(pt_in_vel, JdotQdot) == 0) ){ KDL::Jacobian J = this->arm_.getJacobian(); KDL::Twist xdotdot; // Xdd = Jd * qd + J * qdd for(unsigned int i = 0; i < 6; ++i ) xdotdot(i) = JdotQdot(i) + J.data(i) * this->pt_in_.qdotdot(i); this->traj_pt_out_ = KDL::FrameAcc(traj_pt_vel.GetFrame(), traj_pt_vel.GetTwist(), xdotdot); // Send cartesian trajectory point to the controller this->port_traj_pt_out_.write(this->traj_pt_out_); this->port_traj_joint_out_.write(this->pt_in_); // Increment counter this->traj_pt_nb_++; } }
int SNS_IK::CartToJntVel(const KDL::JntArray& q_in, const KDL::Twist& v_in, const KDL::JntArray& q_bias, const std::vector<std::string>& biasNames, const KDL::JntArray& q_vel_bias, KDL::JntArray& qdot_out) { if (!m_initialized) { ROS_ERROR("SNS_IK was not properly initialized with a valid chain or limits."); return -1; } KDL::Jacobian jacobian; jacobian.resize(q_in.rows()); if (m_jacobianSolver->JntToJac(q_in, jacobian) < 0) { std::cout << "JntToJac failed" << std::endl; return -1; } std::vector<Task> sot; Task task; task.jacobian = jacobian.data; task.desired = VectorD::Zero(6); // twistEigenToKDL for(size_t i = 0; i < 6; i++) task.desired(i) = v_in[i]; sot.push_back(task); // Calculate the nullspace goal as a configuration-space task. // Creates a task Jacobian which maps the provided nullspace joints to // the full joint state. if (q_bias.rows()) { Task task2; std::vector<int> indicies; if (!nullspaceBiasTask(q_bias, biasNames, &(task2.jacobian), &indicies)) { ROS_ERROR("Could not create nullspace bias task"); return -1; } task2.desired = VectorD::Zero(q_bias.rows()); for (size_t ii = 0; ii < q_bias.rows(); ++ii) { // This calculates a "nullspace velocity". // There is an arbitrary scale factor which will be set by the max scale factor. task2.desired(ii) = m_nullspaceGain * (q_bias(ii) - q_in(indicies[ii])) / m_loopPeriod; // TODO: may want to limit the NS velocity to 70-90% of max joint velocity } sot.push_back(task2); } // Bias the joint velocities // If the bias is the previous joint velocities, this is velocity damping if(q_vel_bias.rows() == q_in.rows()) { Task task2; task2.jacobian = MatrixD::Identity(q_vel_bias.rows(), q_vel_bias.rows()); task2.desired = VectorD::Zero(q_vel_bias.rows()); for (size_t ii = 0; ii < q_vel_bias.rows(); ++ii) { task2.desired(ii) = q_vel_bias(ii); } sot.push_back(task2); } return m_ik_vel_solver->getJointVelocity(&qdot_out.data, sot, q_in.data); }
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; } }
vpMatrix convertKDLJacToVispMat(const KDL::Jacobian& kdlJac) { vpMatrix vispMat(kdlJac.rows(), kdlJac.columns()); convertKDLJacToVispMat(kdlJac, vispMat); return vispMat; }
int main(int argc, char *argv[]) { ros::init(argc, argv, "kinematics_publisher"); ros::NodeHandle nh; ros::Publisher joint_publisher; ros::Publisher frame_publisher; joint_publisher = nh.advertise<sensor_msgs::JointState>("kinematics/joint_states", 1, true); frame_publisher = nh.advertise<geometry_msgs::PoseStamped>("/frame_result", 1, true); std::vector<std::string> names; names.resize(2); names.at(0) = "base_to_body"; names.at(1) = "body_to_head"; sensor_msgs::JointState my_joints; my_joints.name = names; my_joints.position.resize(2); my_joints.position.at(0) = 0.2; my_joints.position.at(1) = 0,2; joint_publisher.publish(my_joints); ros::spinOnce(); std::cin.get(); bool exit_value; KDL::Tree my_tree; urdf::Model my_model; if (!kdl_parser::treeFromParam("/robot_description", my_tree)){ ROS_ERROR("Failed to construct kdl tree"); return false; } std::cout << "Number of links of the urdf:" << my_tree.getNrOfSegments() << std::endl; std::cout << "Number of Joints of the urdf:" << my_tree.getNrOfJoints() << std::endl; KDL::Chain my_chain; my_tree.getChain("world", "webcam", my_chain); std::cout << "Number of links of the CHAIN:" << my_chain.getNrOfSegments() << std::endl; std::cout << "Number of Joints of the CHAIN:" << my_chain.getNrOfJoints() << std::endl; KDL::ChainFkSolverPos_recursive fksolver(my_chain); KDL::JntArray q(my_chain.getNrOfJoints()); q(0) = my_joints.position.at(0); q(1) = my_joints.position.at(1); KDL::Frame webcam; fksolver.JntToCart(q,webcam); geometry_msgs::Pose webcam_pose; tf::poseKDLToMsg(webcam, webcam_pose); geometry_msgs::PoseStamped webcam_stamped; webcam_stamped.pose = webcam_pose; webcam_stamped.header.frame_id = "world"; webcam_stamped.header.stamp = ros::Time::now(); frame_publisher.publish(webcam_stamped); ros::spinOnce(); KDL::ChainJntToJacSolver jacobian_solver(my_chain); KDL::Jacobian J; J.resize(2); jacobian_solver.JntToJac(q, J); std::cout << J.data << std::endl; webcam.M.DoRotY(-20*KDL::deg2rad); tf::poseKDLToMsg(webcam, webcam_pose); webcam_stamped.pose = webcam_pose; webcam_stamped.header.stamp = ros::Time::now(); frame_publisher.publish(webcam_stamped); ros::spinOnce(); std::cin.get(); KDL::ChainIkSolverVel_pinv ik_solver_vel(my_chain); KDL::ChainIkSolverPos_NR ik_solver_pos(my_chain,fksolver,ik_solver_vel); KDL::JntArray q_sol(2); ik_solver_pos.CartToJnt(q, webcam,q_sol); my_joints.position.at(0) = q_sol(0); my_joints.position.at(1) = q_sol(1); joint_publisher.publish(my_joints); ros::spinOnce(); std::cin.get(); KDL::Vector g (0.0,0.0,-9.81); KDL::ChainDynParam my_chain_params(my_chain, g); KDL::ChainIdSolver_RNE id_solver(my_chain, g); KDL::JntArray G(2); my_chain_params.JntToGravity(q_sol,G); KDL::JntSpaceInertiaMatrix H(2); my_chain_params.JntToMass(q_sol, H); KDL::JntArray C(2); KDL::JntArray q_dot(2); q_dot(0) = 0.02; q_dot(1) = 0.04; my_chain_params.JntToCoriolis(q_sol,q_dot, C); std::cout << "g(q): " <<G.data << std::endl; std::cout << "M(q): " <<H.data << std::endl; std::cout << "C(q,q_dot): " <<C.data << std::endl; KDL::JntArray q_dotdot(2); q_dotdot(0) = -0.02; q_dotdot(1) = 0.02; KDL::Wrenches f(4); KDL::JntArray tau(2); id_solver.CartToJnt(q_sol, q_dot, q_dotdot, f, tau); std::cout << "Tau: " <<tau.data << std::endl; std::cin.get(); moveit::planning_interface::MoveGroup my_group("webcam_robot"); my_group.setNamedTarget("look_left"); my_group.move(); return false; }