30 ROS_ERROR(
"Failed to construct kdl tree");
35 if (
chain_.getNrOfJoints() == 0)
37 ROS_ERROR(
"Failed to initialize kinematic chain");
41 for (
unsigned int i = 0; i <
chain_.getNrOfSegments(); i++)
46 if (
chain_.getSegment(i).getJoint().getType() != KDL::Joint::None)
60 if (!model.
initParam(
"/robot_description"))
62 ROS_ERROR(
"Failed to parse urdf file for JointLimits");
66 for (
unsigned int i = 0; i <
ext_dof_; i++)
71 limits_acc_.push_back(std::numeric_limits<double>::max());
80 KDL::Jacobian jac_full;
83 Eigen::Matrix<double, 6, Eigen::Dynamic> jac_ext;
88 for (
unsigned int i = 0; i <
chain_.getNrOfSegments(); i++)
90 KDL::Joint joint =
chain_.getSegment(i).getJoint();
91 double eps_type = -1.0;
93 switch (joint.getType())
96 case KDL::Joint::RotAxis:
97 case KDL::Joint::RotX:
98 case KDL::Joint::RotY:
99 case KDL::Joint::RotZ:
103 case KDL::Joint::TransAxis:
104 case KDL::Joint::TransX:
105 case KDL::Joint::TransY:
106 case KDL::Joint::TransZ:
110 case KDL::Joint::None:
123 KDL::Frame eb_frame_ct, cb_frame_eb;
145 Eigen::Quaterniond quat_cb;
147 Eigen::Matrix3d rot_cb = quat_cb.toRotationMatrix();
150 Eigen::Vector3d axis_eb(joint.JointAxis().x(), joint.JointAxis().y(), joint.JointAxis().z());
151 Eigen::Vector3d axis_cb = quat_cb * axis_eb;
155 Eigen::Vector3d p_eb(eb_frame_ct.p.x(), eb_frame_ct.p.y(), eb_frame_ct.p.z());
156 Eigen::Vector3d p_cb = quat_cb * p_eb;
157 Eigen::Vector3d axis_cross_p_cb = axis_cb.cross(p_cb);
160 jac_ext(0, k) = eps_type * axis_cb(0) + (1.0 - eps_type) * axis_cross_p_cb(0);
161 jac_ext(1, k) = eps_type * axis_cb(0) + (1.0 - eps_type) * axis_cross_p_cb(1);
162 jac_ext(2, k) = eps_type * axis_cb(0) + (1.0 - eps_type) * axis_cross_p_cb(2);
163 jac_ext(3, k) = eps_type * 0.0 + (1.0 - eps_type) * axis_cb(0);
164 jac_ext(4, k) = eps_type * 0.0 + (1.0 - eps_type) * axis_cb(1);
165 jac_ext(5, k) = eps_type * 0.0 + (1.0 - eps_type) * axis_cb(2);
174 jac_full_matrix.resize(6, jac_chain.data.cols() + jac_ext.cols());
175 jac_full_matrix << jac_chain.data, jac_ext;
176 jac_full.resize(jac_chain.data.cols() + jac_ext.cols());
177 jac_full.data << jac_full_matrix;
185 unsigned int chain_dof = joint_states.
current_q_.rows();
191 for (
unsigned int i = 0; i< chain_dof; i++)
198 for (
unsigned int i = 0; i <
ext_dof_; i++)
212 for (
unsigned int i = 0; i <
ext_dof_; i++)
224 std_msgs::Float64MultiArray command_msg;
226 for (
unsigned int i = 0; i <
ext_dof_; i++)
228 command_msg.data.push_back(q_dot_ik(
params_.
dof+i));
240 for (
unsigned int i = 0; i <
ext_dof_; i++)
242 q_temp(i) =
msg->position[i];
243 q_dot_temp(i) =
msg->velocity[i];