kinematic_extension_urdf.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #include <string>
19 #include <limits>
22 
23 /* BEGIN KinematicExtensionURDF ********************************************************************************************/
25 {
27  KDL::Tree tree;
28  if (!kdl_parser::treeFromParam("robot_description", tree))
29  {
30  ROS_ERROR("Failed to construct kdl tree");
31  return false;
32  }
33 
34  tree.getChain(ext_base_, ext_tip_, chain_);
35  if (chain_.getNrOfJoints() == 0)
36  {
37  ROS_ERROR("Failed to initialize kinematic chain");
38  return false;
39  }
40 
41  for (unsigned int i = 0; i < chain_.getNrOfSegments(); i++)
42  {
43  ROS_DEBUG_STREAM("Segment[" << i << "] Name : " << chain_.getSegment(i).getName());
44  ROS_DEBUG_STREAM("Joint[" << i << "] Name: " << chain_.getSegment(i).getJoint().getName());
45  ROS_DEBUG_STREAM("Joint[" << i << "] Type: " << chain_.getSegment(i).getJoint().getTypeName());
46  if (chain_.getSegment(i).getJoint().getType() != KDL::Joint::None)
47  {
48  ROS_DEBUG_STREAM("Adding Joint " << chain_.getSegment(i).getJoint().getName());
49  joint_names_.push_back(chain_.getSegment(i).getJoint().getName());
50  }
51  }
52  this->ext_dof_ = chain_.getNrOfJoints();
53  this->joint_states_.last_q_.resize(ext_dof_);
54  this->joint_states_.last_q_dot_.resize(ext_dof_);
55  this->joint_states_.current_q_.resize(ext_dof_);
57 
59  urdf::Model model;
60  if (!model.initParam("/robot_description"))
61  {
62  ROS_ERROR("Failed to parse urdf file for JointLimits");
63  return false;
64  }
65 
66  for (unsigned int i = 0; i < ext_dof_; i++)
67  {
68  limits_max_.push_back(model.getJoint(joint_names_[i])->limits->upper);
69  limits_min_.push_back(model.getJoint(joint_names_[i])->limits->lower);
70  limits_vel_.push_back(model.getJoint(joint_names_[i])->limits->velocity);
71  limits_acc_.push_back(std::numeric_limits<double>::max());
72  }
73 
74  return true;
75 }
76 
77 KDL::Jacobian KinematicExtensionURDF::adjustJacobian(const KDL::Jacobian& jac_chain)
78 {
80  KDL::Jacobian jac_full;
81 
82  // jacobian matrix for the extension
83  Eigen::Matrix<double, 6, Eigen::Dynamic> jac_ext;
84  jac_ext.resize(6, ext_dof_);
85  jac_ext.setZero();
86 
87  unsigned int k = 0;
88  for (unsigned int i = 0; i < chain_.getNrOfSegments(); i++)
89  {
90  KDL::Joint joint = chain_.getSegment(i).getJoint();
91  double eps_type = -1.0; // joint_type selector: 0.0 -> rot_axis, 1.0 -> lin_axis
92 
93  switch (joint.getType())
94  {
95  // revolute axis
96  case KDL::Joint::RotAxis:
97  case KDL::Joint::RotX:
98  case KDL::Joint::RotY:
99  case KDL::Joint::RotZ:
100  eps_type = 0.0;
101  break;
102  // linear axis
103  case KDL::Joint::TransAxis:
104  case KDL::Joint::TransX:
105  case KDL::Joint::TransY:
106  case KDL::Joint::TransZ:
107  eps_type = 1.0;
108  break;
109  // fixed axis
110  case KDL::Joint::None:
111  default:
112  break;
113  }
114 
115  if (eps_type < 0.0)
116  {
117  ROS_DEBUG_STREAM("Not considering " << joint.getName() << " in jac_ext");
118  continue;
119  }
120 
122  tf::StampedTransform eb_transform_ct, cb_transform_eb;
123  KDL::Frame eb_frame_ct, cb_frame_eb;
124  try
125  {
126  ros::Time now = ros::Time::now();
127  tf_listener_.waitForTransform(chain_.getSegment(i).getName(), params_.chain_tip_link, now, ros::Duration(0.5));
128  tf_listener_.lookupTransform(chain_.getSegment(i).getName(), params_.chain_tip_link, now, eb_transform_ct);
129 
130  tf_listener_.waitForTransform(params_.chain_base_link, chain_.getSegment(i).getName(), now, ros::Duration(0.5));
131  tf_listener_.lookupTransform(params_.chain_base_link, chain_.getSegment(i).getName(), now, cb_transform_eb);
132  }
133  catch (tf::TransformException& ex)
134  {
135  ROS_ERROR("%s", ex.what());
136  }
137 
138  eb_frame_ct.p = KDL::Vector(eb_transform_ct.getOrigin().x(), eb_transform_ct.getOrigin().y(), eb_transform_ct.getOrigin().z());
139  eb_frame_ct.M = KDL::Rotation::Quaternion(eb_transform_ct.getRotation().x(), eb_transform_ct.getRotation().y(), eb_transform_ct.getRotation().z(), eb_transform_ct.getRotation().w());
140 
141  cb_frame_eb.p = KDL::Vector(cb_transform_eb.getOrigin().x(), cb_transform_eb.getOrigin().y(), cb_transform_eb.getOrigin().z());
142  cb_frame_eb.M = KDL::Rotation::Quaternion(cb_transform_eb.getRotation().x(), cb_transform_eb.getRotation().y(), cb_transform_eb.getRotation().z(), cb_transform_eb.getRotation().w());
143 
144  // rotation from base_frame of primary chain to base_frame of extension (eb)
145  Eigen::Quaterniond quat_cb;
146  tf::quaternionKDLToEigen(cb_frame_eb.M, quat_cb);
147  Eigen::Matrix3d rot_cb = quat_cb.toRotationMatrix();
148 
150  Eigen::Vector3d axis_eb(joint.JointAxis().x(), joint.JointAxis().y(), joint.JointAxis().z()); // axis wrt eb
151  Eigen::Vector3d axis_cb = quat_cb * axis_eb; // transform to cb
152 
154  // vector from base_frame of extension (eb) to endeffector (ct)
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; // transform to cb
157  Eigen::Vector3d axis_cross_p_cb = axis_cb.cross(p_cb); // tangential velocity
158 
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);
166  k++;
167  }
168 
169  // scale with extension_ratio
170  jac_ext *= params_.extension_ratio;
171 
172  // combine Jacobian of primary chain and extension
173  Matrix6Xd_t jac_full_matrix;
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;
178 
179  return jac_full;
180 }
181 
183 {
184  JointStates js;
185  unsigned int chain_dof = joint_states.current_q_.rows();
186  js.current_q_.resize(chain_dof + ext_dof_);
187  js.last_q_.resize(chain_dof + ext_dof_);
188  js.current_q_dot_.resize(chain_dof + ext_dof_);
189  js.last_q_dot_.resize(chain_dof + ext_dof_);
190 
191  for (unsigned int i = 0; i< chain_dof; i++)
192  {
193  js.current_q_(i) = joint_states.current_q_(i);
194  js.last_q_(i) = joint_states.last_q_(i);
195  js.current_q_dot_(i) = joint_states.current_q_dot_(i);
196  js.last_q_dot_(i) = joint_states.last_q_dot_(i);
197  }
198  for (unsigned int i = 0; i < ext_dof_; i++)
199  {
200  js.current_q_(chain_dof + i) = this->joint_states_.current_q_(i);
201  js.last_q_(chain_dof + i) = this->joint_states_.last_q_(i);
202  js.current_q_dot_(chain_dof + i) = this->joint_states_.current_q_dot_(i);
203  js.last_q_dot_(chain_dof + i) = this->joint_states_.last_q_dot_(i);
204  }
205 
206  return js;
207 }
208 
210 {
211  LimiterParams lp = limiter_params;
212  for (unsigned int i = 0; i < ext_dof_; i++)
213  {
214  lp.limits_max.push_back(limits_max_[i]);
215  lp.limits_min.push_back(limits_min_[i]);
216  lp.limits_vel.push_back(limits_vel_[i]);
217  lp.limits_acc.push_back(limits_acc_[i]);
218  }
219  return lp;
220 }
221 
222 void KinematicExtensionURDF::processResultExtension(const KDL::JntArray& q_dot_ik)
223 {
224  std_msgs::Float64MultiArray command_msg;
225 
226  for (unsigned int i = 0; i < ext_dof_; i++)
227  {
228  command_msg.data.push_back(q_dot_ik(params_.dof+i));
229  }
230 
231  command_pub_.publish(command_msg);
232 }
233 
234 void KinematicExtensionURDF::jointstateCallback(const sensor_msgs::JointState::ConstPtr& msg)
235 {
236  KDL::JntArray q_temp = this->joint_states_.current_q_;
237  KDL::JntArray q_dot_temp = this->joint_states_.current_q_dot_;
238 
239  // ToDo: Do we need more robust parsing/handling?
240  for (unsigned int i = 0; i < ext_dof_; i++)
241  {
242  q_temp(i) = msg->position[i];
243  q_dot_temp(i) = msg->velocity[i];
244  }
245 
248  this->joint_states_.current_q_ = q_temp;
249  this->joint_states_.current_q_dot_ = q_dot_temp;
250 }
251 /* END KinematicExtensionURDF ********************************************************************************************/
tf::Transform::getRotation
Quaternion getRotation() const
JointStates::last_q_dot_
KDL::JntArray last_q_dot_
Definition: cob_twist_controller_data_types.h:117
KinematicExtensionURDF::processResultExtension
virtual void processResultExtension(const KDL::JntArray &q_dot_ik)
Definition: kinematic_extension_urdf.cpp:222
LimiterParams::limits_min
std::vector< double > limits_min
Definition: cob_twist_controller_data_types.h:189
KinematicExtensionURDF::limits_min_
std::vector< double > limits_min_
Definition: kinematic_extension_urdf.h:63
KinematicExtensionURDF::limits_max_
std::vector< double > limits_max_
Definition: kinematic_extension_urdf.h:62
msg
msg
KinematicExtensionURDF::limits_acc_
std::vector< double > limits_acc_
Definition: kinematic_extension_urdf.h:65
KinematicExtensionBase::tf_listener_
tf::TransformListener tf_listener_
Definition: kinematic_extension_base.h:48
TwistControllerParams::extension_ratio
double extension_ratio
Definition: cob_twist_controller_data_types.h:286
JointStates::last_q_
KDL::JntArray last_q_
Definition: cob_twist_controller_data_types.h:115
LimiterParams::limits_acc
std::vector< double > limits_acc
Definition: cob_twist_controller_data_types.h:191
TwistControllerParams::dof
uint8_t dof
Definition: cob_twist_controller_data_types.h:255
JointStates::current_q_dot_
KDL::JntArray current_q_dot_
Definition: cob_twist_controller_data_types.h:116
KinematicExtensionURDF::adjustLimiterParams
virtual LimiterParams adjustLimiterParams(const LimiterParams &limiter_params)
Definition: kinematic_extension_urdf.cpp:209
KinematicExtensionURDF::chain_
KDL::Chain chain_
Definition: kinematic_extension_urdf.h:58
urdf::Model
tf::StampedTransform
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
KinematicExtensionURDF::joint_states_
JointStates joint_states_
Definition: kinematic_extension_urdf.h:61
tree
tree
KinematicExtensionBase::params_
const TwistControllerParams & params_
Definition: kinematic_extension_base.h:49
KinematicExtensionURDF::limits_vel_
std::vector< double > limits_vel_
Definition: kinematic_extension_urdf.h:64
KinematicExtensionURDF::ext_base_
std::string ext_base_
Definition: kinematic_extension_urdf.h:56
KinematicExtensionURDF::adjustJacobian
virtual KDL::Jacobian adjustJacobian(const KDL::Jacobian &jac_chain)
Definition: kinematic_extension_urdf.cpp:77
KinematicExtensionURDF::command_pub_
ros::Publisher command_pub_
Definition: kinematic_extension_urdf.h:53
LimiterParams::limits_vel
std::vector< double > limits_vel
Definition: cob_twist_controller_data_types.h:190
ROS_ERROR
#define ROS_ERROR(...)
kdl_parser::treeFromParam
KDL_PARSER_PUBLIC bool treeFromParam(const std::string &param, KDL::Tree &tree)
tf::Transformer::waitForTransform
bool waitForTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
tf::quaternionKDLToEigen
void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e)
Matrix6Xd_t
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd_t
Definition: cob_twist_controller_data_types.h:451
LimiterParams
Definition: cob_twist_controller_data_types.h:162
KinematicExtensionURDF::ext_dof_
unsigned int ext_dof_
Definition: kinematic_extension_urdf.h:59
ros::Time
KinematicExtensionURDF::jointstateCallback
void jointstateCallback(const sensor_msgs::JointState::ConstPtr &msg)
Definition: kinematic_extension_urdf.cpp:234
urdf::Model::initParam
URDF_EXPORT bool initParam(const std::string &param)
KinematicExtensionURDF::ext_tip_
std::string ext_tip_
Definition: kinematic_extension_urdf.h:57
KinematicExtensionURDF::joint_names_
std::vector< std::string > joint_names_
Definition: kinematic_extension_urdf.h:60
KinematicExtensionURDF::adjustJointStates
virtual JointStates adjustJointStates(const JointStates &joint_states)
Definition: kinematic_extension_urdf.cpp:182
TwistControllerParams::chain_tip_link
std::string chain_tip_link
Definition: cob_twist_controller_data_types.h:257
LimiterParams::limits_max
std::vector< double > limits_max
Definition: cob_twist_controller_data_types.h:188
JointStates
Definition: cob_twist_controller_data_types.h:112
eigen_kdl.h
JointStates::current_q_
KDL::JntArray current_q_
Definition: cob_twist_controller_data_types.h:114
kinematic_extension_urdf.h
tf::Transformer::lookupTransform
void lookupTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, StampedTransform &transform) const
TwistControllerParams::chain_base_link
std::string chain_base_link
Definition: cob_twist_controller_data_types.h:256
tf2::TransformException
ros::Duration
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
KinematicExtensionURDF::initExtension
bool initExtension()
Definition: kinematic_extension_urdf.cpp:24
ros::Time::now
static Time now()


cob_twist_controller
Author(s): Felix Messmer , Marco Bezzon , Christoph Mark , Francisco Moreno
autogenerated on Mon May 1 2023 02:44:43