manipulation_module.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
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  * manipulation_module.cpp
19  *
20  * Created on: December 13, 2016
21  * Author: SCH
22  */
23 
25 
26 using namespace thormang3;
27 
29  : control_cycle_sec_(0.008),
30  is_moving_(false),
31  ik_solving_(false),
32  arm_angle_display_(false)
33 {
34  enable_ = false;
35  module_name_ = "manipulation_module";
37 
38  /* arm */
39  result_["r_arm_sh_p1"] = new robotis_framework::DynamixelState();
40  result_["l_arm_sh_p1"] = new robotis_framework::DynamixelState();
41  result_["r_arm_sh_r"] = new robotis_framework::DynamixelState();
42  result_["l_arm_sh_r"] = new robotis_framework::DynamixelState();
43  result_["r_arm_sh_p2"] = new robotis_framework::DynamixelState();
44  result_["l_arm_sh_p2"] = new robotis_framework::DynamixelState();
45  result_["r_arm_el_y"] = new robotis_framework::DynamixelState();
46  result_["l_arm_el_y"] = new robotis_framework::DynamixelState();
47  result_["r_arm_wr_r"] = new robotis_framework::DynamixelState();
48  result_["l_arm_wr_r"] = new robotis_framework::DynamixelState();
49  result_["r_arm_wr_y"] = new robotis_framework::DynamixelState();
50  result_["l_arm_wr_y"] = new robotis_framework::DynamixelState();
51  result_["r_arm_wr_p"] = new robotis_framework::DynamixelState();
52  result_["l_arm_wr_p"] = new robotis_framework::DynamixelState();
53  result_["torso_y"] = new robotis_framework::DynamixelState();
54 
55  /* arm */
56  joint_name_to_id_["r_arm_sh_p1"] = 1;
57  joint_name_to_id_["l_arm_sh_p1"] = 2;
58  joint_name_to_id_["r_arm_sh_r"] = 3;
59  joint_name_to_id_["l_arm_sh_r"] = 4;
60  joint_name_to_id_["r_arm_sh_p2"] = 5;
61  joint_name_to_id_["l_arm_sh_p2"] = 6;
62  joint_name_to_id_["r_arm_el_y"] = 7;
63  joint_name_to_id_["l_arm_el_y"] = 8;
64  joint_name_to_id_["r_arm_wr_r"] = 9;
65  joint_name_to_id_["l_arm_wr_r"] = 10;
66  joint_name_to_id_["r_arm_wr_y"] = 11;
67  joint_name_to_id_["l_arm_wr_y"] = 12;
68  joint_name_to_id_["r_arm_wr_p"] = 13;
69  joint_name_to_id_["l_arm_wr_p"] = 14;
70  joint_name_to_id_["torso_y"] = 27;
71 
72  /* etc */
73  joint_name_to_id_["r_arm_end"] = 35;
74  joint_name_to_id_["l_arm_end"] = 34;
75 
76  /* parameter */
77  present_joint_position_ = Eigen::VectorXd::Zero(MAX_JOINT_ID+1);
78  goal_joint_position_ = Eigen::VectorXd::Zero(MAX_JOINT_ID+1);
79  init_joint_position_ = Eigen::VectorXd::Zero(MAX_JOINT_ID+1);
80 
81  ik_id_start_ = 0;
82  ik_id_end_ = 0;
83 
84  ik_target_position_ = Eigen::MatrixXd::Zero(3,1);
85  ik_weight_ = Eigen::MatrixXd::Zero(MAX_JOINT_ID+1, 1);
86  ik_weight_.fill(1.0);
87 
89 }
90 
92 {
93  queue_thread_.join();
94 }
95 
96 void ManipulationModule::initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
97 {
98  control_cycle_sec_ = control_cycle_msec * 0.001;
99  queue_thread_ = boost::thread(boost::bind(&ManipulationModule::queueThread, this));
100 
101  ros::NodeHandle ros_node;
102 
103  /* publish topics */
104 
105  // for gui
106  status_msg_pub_ = ros_node.advertise<robotis_controller_msgs::StatusMsg>("/robotis/status", 1);
107  movement_done_pub_ = ros_node.advertise<std_msgs::String>("/robotis/movement_done", 1);
108 
109  std::string _path = ros::package::getPath("thormang3_manipulation_module") + "/config/ik_weight.yaml";
110  parseData(_path);
111 }
112 
113 void ManipulationModule::parseData(const std::string &path)
114 {
115  YAML::Node doc;
116  try
117  {
118  // load yaml
119  doc = YAML::LoadFile(path.c_str());
120  } catch (const std::exception& e)
121  {
122  ROS_ERROR("Fail to load yaml file.");
123  return;
124  }
125 
126  YAML::Node ik_weight_node = doc["weight_value"];
127  for (YAML::iterator it = ik_weight_node.begin(); it != ik_weight_node.end(); ++it)
128  {
129  int id = it->first.as<int>();
130  double value = it->second.as<double>();
131 
132  ik_weight_.coeffRef(id, 0) = value;
133  }
134 }
135 
136 void ManipulationModule::parseIniPoseData(const std::string &path)
137 {
138  YAML::Node doc;
139  try
140  {
141  // load yaml
142  doc = YAML::LoadFile(path.c_str());
143  } catch (const std::exception& e)
144  {
145  ROS_ERROR("Fail to load yaml file.");
146  return;
147  }
148 
149  // parse movement time
150  double mov_time = doc["mov_time"].as<double>();
151  mov_time_ = mov_time;
152 
153  // parse target pose
154  YAML::Node tar_pose_node = doc["tar_pose"];
155  for (YAML::iterator it = tar_pose_node.begin(); it != tar_pose_node.end(); ++it)
156  {
157  int id = it->first.as<int>();
158  double value = it->second.as<double>();
159 
160  init_joint_position_(id) = value * DEGREE2RADIAN;
161  }
162 
163  all_time_steps_ = int(mov_time_ / control_cycle_sec_) + 1;
165 }
166 
168 {
169  ros::NodeHandle ros_node;
170  ros::CallbackQueue callback_queue;
171 
172  ros_node.setCallbackQueue(&callback_queue);
173 
174  /* subscribe topics */
175  ros::Subscriber ini_pose_msg_sub = ros_node.subscribe("/robotis/manipulation/ini_pose_msg", 5,
177  ros::Subscriber joint_pose_msg_sub = ros_node.subscribe("/robotis/manipulation/joint_pose_msg", 5,
179  ros::Subscriber kinematics_pose_msg_sub = ros_node.subscribe("/robotis/manipulation/kinematics_pose_msg", 5,
181 
182  /* service */
183  ros::ServiceServer get_joint_pose_server = ros_node.advertiseService("/robotis/manipulation/get_joint_pose",
185  ros::ServiceServer get_kinematics_pose_server = ros_node.advertiseService("/robotis/manipulation/get_kinematics_pose",
187 
189  while(ros_node.ok())
190  callback_queue.callAvailable(duration);
191 }
192 
193 void ManipulationModule::initPoseMsgCallback(const std_msgs::String::ConstPtr& msg)
194 {
195  if (enable_ == false)
196  return;
197 
198  if (is_moving_ == false)
199  {
200  if (msg->data == "ini_pose")
201  {
202  // parse initial pose
203  std::string ini_pose_path = ros::package::getPath("thormang3_manipulation_module") + "/config/ini_pose.yaml";
204  parseIniPoseData(ini_pose_path);
205 
206  traj_generate_tread_ = new boost::thread(boost::bind(&ManipulationModule::initPoseTrajGenerateProc, this));
207  delete traj_generate_tread_;
208  }
209  }
210  else
211  {
212  ROS_INFO("previous task is alive");
213  }
214 
215  movement_done_msg_.data = "manipulation_init";
216 
217  return;
218 }
219 
220 bool ManipulationModule::getJointPoseCallback(thormang3_manipulation_module_msgs::GetJointPose::Request &req,
221  thormang3_manipulation_module_msgs::GetJointPose::Response &res)
222 {
223  if (enable_ == false)
224  return false;
225 
226  for (int name_index = 1; name_index <= MAX_JOINT_ID; name_index++)
227  {
228  if (robotis_->thormang3_link_data_[name_index]->name_ == req.joint_name)
229  {
230  res.joint_value = goal_joint_position_(name_index);
231  return true;
232  }
233  }
234 
235  return false;
236 }
237 
238 bool ManipulationModule::getKinematicsPoseCallback(thormang3_manipulation_module_msgs::GetKinematicsPose::Request &req,
239  thormang3_manipulation_module_msgs::GetKinematicsPose::Response &res)
240 {
241  if (enable_ == false)
242  return false;
243 
244  int end_index;
245 
246  if (req.group_name == "left_arm")
247  end_index = ID_L_ARM_END;
248  else if (req.group_name == "right_arm")
249  end_index = ID_R_ARM_END;
250  else if (req.group_name == "left_arm_with_torso")
251  end_index = ID_L_ARM_END;
252  else if (req.group_name == "right_arm_with_torso")
253  end_index = ID_R_ARM_END;
254  else
255  return false;
256 
257  res.group_pose.position.x = robotis_->thormang3_link_data_[end_index]->position_.coeff(0, 0);
258  res.group_pose.position.y = robotis_->thormang3_link_data_[end_index]->position_.coeff(1, 0);
259  res.group_pose.position.z = robotis_->thormang3_link_data_[end_index]->position_.coeff(2, 0);
260 
262 
263  res.group_pose.orientation.w = quaternion.w();
264  res.group_pose.orientation.x = quaternion.x();
265  res.group_pose.orientation.y = quaternion.y();
266  res.group_pose.orientation.z = quaternion.z();
267 
268  return true;
269 }
270 
271 void ManipulationModule::kinematicsPoseMsgCallback(const thormang3_manipulation_module_msgs::KinematicsPose::ConstPtr& msg)
272 {
273  if (enable_ == false)
274  return;
275 
276  movement_done_msg_.data = "manipulation";
277 
279 
280  if (goal_kinematics_pose_msg_.name == "left_arm")
281  {
284  }
285  else if (goal_kinematics_pose_msg_.name == "right_arm")
286  {
289  }
290  else if (goal_kinematics_pose_msg_.name == "left_arm_with_torso")
291  {
294  }
295  else if (goal_kinematics_pose_msg_.name == "right_arm_with_torso")
296  {
299  }
300 
301  if (is_moving_ == false)
302  {
303  traj_generate_tread_ = new boost::thread(boost::bind(&ManipulationModule::taskTrajGenerateProc, this));
304  delete traj_generate_tread_;
305  }
306  else
307  {
308  ROS_INFO("previous task is alive");
309  }
310 
311  return;
312 }
313 
314 void ManipulationModule::jointPoseMsgCallback(const thormang3_manipulation_module_msgs::JointPose::ConstPtr& msg)
315 {
316  if (enable_ == false)
317  return;
318 
319  goal_joint_pose_msg_ = *msg;
320 
321  if (is_moving_ == false)
322  {
323  traj_generate_tread_ = new boost::thread(boost::bind(&ManipulationModule::jointTrajGenerateProc, this));
324  delete traj_generate_tread_;
325  }
326  else
327  ROS_INFO("previous task is alive");
328 
329  return;
330 }
331 
333 {
334  for (int id = 1; id <= MAX_JOINT_ID; id++)
335  {
336  double ini_value = goal_joint_position_(id);
337  double tar_value = init_joint_position_(id);
338 
339  Eigen::MatrixXd tra = robotis_framework::calcMinimumJerkTra(ini_value, 0.0, 0.0,
340  tar_value, 0.0, 0.0,
342  mov_time_);
343 
344  goal_joint_tra_.block(0, id, all_time_steps_, 1) = tra;
345  }
346 
347  cnt_ = 0;
348  is_moving_ = true;
349 
350  ROS_INFO("[start] send trajectory");
351 }
352 
354 {
355  if (goal_joint_pose_msg_.time <= 0.0)
356  {
357  /* set movement time */
358  double tol = 10 * DEGREE2RADIAN; // rad per sec
359  double mov_time = 2.0;
360 
362 
363  double ini_value = goal_joint_position_(id);
364  double tar_value = goal_joint_pose_msg_.value;
365  double diff = fabs(tar_value - ini_value);
366 
367  mov_time_ = diff / tol;
368  int _all_time_steps = int(floor((mov_time_ / control_cycle_sec_) + 1.0));
369  mov_time_ = double(_all_time_steps - 1) * control_cycle_sec_;
370 
371  if (mov_time_ < mov_time)
372  mov_time_ = mov_time;
373  }
374  else
375  {
377  }
378 
380 
382 
383  /* calculate joint trajectory */
384  for (int id = 1; id <= MAX_JOINT_ID; id++)
385  {
386  double ini_value = goal_joint_position_(id);
387  double tar_value = goal_joint_position_(id);
388 
390  tar_value = goal_joint_pose_msg_.value;
391 
392  Eigen::MatrixXd tra = robotis_framework::calcMinimumJerkTra(ini_value, 0.0, 0.0, tar_value, 0.0, 0.0,
394  mov_time_);
395 
396  goal_joint_tra_.block(0, id, all_time_steps_, 1) = tra;
397  }
398 
399  cnt_ = 0;
400  is_moving_ = true;
401 
402  ROS_INFO("[start] send trajectory");
403 }
404 
406 {
407  if (goal_kinematics_pose_msg_.time <= 0.0)
408  {
409  /* set movement time */
410  double tol = 0.1; // m per sec
411  double mov_time = 2.0;
412 
413  double diff = sqrt(
414  pow(robotis_->thormang3_link_data_[ik_id_end_]->position_.coeff(0,0) - goal_kinematics_pose_msg_.pose.position.x, 2)
415  + pow(robotis_->thormang3_link_data_[ik_id_end_]->position_.coeff(1,0) - goal_kinematics_pose_msg_.pose.position.y, 2)
416  + pow(robotis_->thormang3_link_data_[ik_id_end_]->position_.coeff(2,0) - goal_kinematics_pose_msg_.pose.position.z, 2)
417  );
418 
419  mov_time_ = diff / tol;
420  int all_time_steps = int(floor((mov_time_ / control_cycle_sec_) + 1.0));
421  mov_time_ = double(all_time_steps - 1) * control_cycle_sec_;
422 
423  if (mov_time_ < mov_time)
424  mov_time_ = mov_time;
425  }
426  else
427  {
429  }
430 
432  goal_task_tra_.resize(all_time_steps_, 3);
433 
434  /* calculate trajectory */
435  for (int dim = 0; dim < 3; dim++)
436  {
437  double ini_value = robotis_->thormang3_link_data_[ik_id_end_]->position_.coeff(dim, 0);
438  double tar_value;
439  if (dim == 0)
440  tar_value = goal_kinematics_pose_msg_.pose.position.x;
441  else if (dim == 1)
442  tar_value = goal_kinematics_pose_msg_.pose.position.y;
443  else if (dim == 2)
444  tar_value = goal_kinematics_pose_msg_.pose.position.z;
445 
446  Eigen::MatrixXd tra = robotis_framework::calcMinimumJerkTra(ini_value, 0.0, 0.0, tar_value, 0.0, 0.0,
448  mov_time_);
449 
450  goal_task_tra_.block(0, dim, all_time_steps_, 1) = tra;
451  }
452 
453  cnt_ = 0;
454  is_moving_ = true;
455  ik_solving_ = true;
456 
457  ROS_INFO("[start] send trajectory");
458 }
459 
460 void ManipulationModule::setInverseKinematics(int cnt, Eigen::MatrixXd start_rotation)
461 {
462  for (int dim = 0; dim < 3; dim++)
463  ik_target_position_.coeffRef(dim, 0) = goal_task_tra_.coeff(cnt, dim);
464 
465  Eigen::Quaterniond start_quaternion = robotis_framework::convertRotationToQuaternion(start_rotation);
466 
467  Eigen::Quaterniond target_quaternion(goal_kinematics_pose_msg_.pose.orientation.w,
468  goal_kinematics_pose_msg_.pose.orientation.x,
469  goal_kinematics_pose_msg_.pose.orientation.y,
470  goal_kinematics_pose_msg_.pose.orientation.z);
471 
472  double count = (double) cnt / (double) all_time_steps_;
473 
474  Eigen::Quaterniond _quaternion = start_quaternion.slerp(count, target_quaternion);
475 
477 }
478 
479 void ManipulationModule::process(std::map<std::string, robotis_framework::Dynamixel *> dxls,
480  std::map<std::string, double> sensors)
481 {
482  if (enable_ == false)
483  return;
484 
485  /*----- write curr position -----*/
486 
487  for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter = result_.begin();
488  state_iter != result_.end(); state_iter++)
489  {
490  std::string joint_name = state_iter->first;
491 
492  robotis_framework::Dynamixel *dxl = NULL;
493  std::map<std::string, robotis_framework::Dynamixel*>::iterator dxl_it = dxls.find(joint_name);
494  if (dxl_it != dxls.end())
495  dxl = dxl_it->second;
496  else
497  continue;
498 
499  double joint_curr_position = dxl->dxl_state_->present_position_;
500  double joint_goal_position = dxl->dxl_state_->goal_position_;
501 
502  present_joint_position_(joint_name_to_id_[joint_name]) = joint_curr_position;
503  goal_joint_position_(joint_name_to_id_[joint_name]) = joint_goal_position;
504  }
505 
506  /*----- forward kinematics -----*/
507  for (int id = 1; id <= MAX_JOINT_ID; id++)
509 
511 
512  /* ----- send trajectory ----- */
513 
514  if (is_moving_ == true)
515  {
516  if (cnt_ == 0)
517  {
518  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "Start Trajectory");
519 
521  }
522 
523  if (ik_solving_ == true)
524  {
525  /* ----- inverse kinematics ----- */
527 
528  int max_iter = 30;
529  double ik_tol = 1e-3;
530  bool ik_success = robotis_->calcInverseKinematics(ik_id_start_,
531  ik_id_end_,
534  max_iter, ik_tol,
535  ik_weight_);
536 
537  if (ik_success == true)
538  {
539  for (int id = 1; id <= MAX_JOINT_ID; id++)
541  }
542  else
543  {
544  ROS_INFO("----- ik failed -----");
545  ROS_INFO("[end] send trajectory");
546 
547  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "IK Failed");
548 
549  is_moving_ = false;
550  ik_solving_ = false;
551  cnt_ = 0;
552 
553  movement_done_msg_.data = "manipulation_fail";
555  movement_done_msg_.data = "";
556  }
557  }
558  else
559  {
560  for (int id = 1; id <= MAX_JOINT_ID; id++)
562  }
563 
564  cnt_++;
565  }
566 
567  /*----- set joint data -----*/
568  for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter = result_.begin();
569  state_iter != result_.end(); state_iter++)
570  {
571  std::string joint_name = state_iter->first;
572  result_[joint_name]->goal_position_ = goal_joint_position_(joint_name_to_id_[joint_name]);
573  }
574 
575  /*---------- initialize count number ----------*/
576  if (is_moving_ == true)
577  {
578  if (cnt_ >= all_time_steps_)
579  {
580  ROS_INFO("[end] send trajectory");
581 
582  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "End Trajectory");
583 
584  is_moving_ = false;
585  ik_solving_ = false;
586  cnt_ = 0;
587 
589  movement_done_msg_.data = "";
590 
591  if (arm_angle_display_ == true)
592  {
593  ROS_INFO("l_arm_sh_p1 : %f", goal_joint_position_(joint_name_to_id_["l_arm_sh_p1"]) * RADIAN2DEGREE );
594  ROS_INFO("l_arm_sh_r : %f", goal_joint_position_(joint_name_to_id_["l_arm_sh_r"]) * RADIAN2DEGREE );
595  ROS_INFO("l_arm_sh_p2 : %f", goal_joint_position_(joint_name_to_id_["l_arm_sh_p2"]) * RADIAN2DEGREE );
596  ROS_INFO("l_arm_el_y : %f", goal_joint_position_(joint_name_to_id_["l_arm_el_y"]) * RADIAN2DEGREE );
597  ROS_INFO("l_arm_wr_r : %f", goal_joint_position_(joint_name_to_id_["l_arm_wr_r"]) * RADIAN2DEGREE );
598  ROS_INFO("l_arm_wr_y : %f", goal_joint_position_(joint_name_to_id_["l_arm_wr_y"]) * RADIAN2DEGREE );
599  ROS_INFO("l_arm_wr_p : %f", goal_joint_position_(joint_name_to_id_["l_arm_wr_p"]) * RADIAN2DEGREE );
600 
601  ROS_INFO("r_arm_sh_p1 : %f", goal_joint_position_(joint_name_to_id_["r_arm_sh_p1"]) * RADIAN2DEGREE );
602  ROS_INFO("r_arm_sh_r : %f", goal_joint_position_(joint_name_to_id_["r_arm_sh_r"]) * RADIAN2DEGREE );
603  ROS_INFO("r_arm_sh_p2 : %f", goal_joint_position_(joint_name_to_id_["r_arm_sh_p2"]) * RADIAN2DEGREE );
604  ROS_INFO("r_arm_el_y : %f", goal_joint_position_(joint_name_to_id_["r_arm_el_y"]) * RADIAN2DEGREE );
605  ROS_INFO("r_arm_wr_r : %f", goal_joint_position_(joint_name_to_id_["r_arm_wr_r"]) * RADIAN2DEGREE );
606  ROS_INFO("r_arm_wr_y : %f", goal_joint_position_(joint_name_to_id_["r_arm_wr_y"]) * RADIAN2DEGREE );
607  ROS_INFO("r_arm_wr_p : %f", goal_joint_position_(joint_name_to_id_["r_arm_wr_p"]) * RADIAN2DEGREE );
608  }
609 
610  }
611  }
612 }
613 
615 {
616  is_moving_ = false;
617  ik_solving_ = false;
618  cnt_ = 0;
619 
620  return;
621 }
622 
624 {
625  return is_moving_;
626 }
627 
628 void ManipulationModule::publishStatusMsg(unsigned int type, std::string msg)
629 {
630  robotis_controller_msgs::StatusMsg status;
631  status.header.stamp = ros::Time::now();
632  status.type = type;
633  status.module_name = "Manipulation";
634  status.status_msg = msg;
635 
636  status_msg_pub_.publish(status);
637 }
Eigen::MatrixXd orientation_
std::map< std::string, DynamixelState * > result_
#define ID_R_ARM_END
Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, double pos_end, double vel_end, double accel_end, double smp_time, double mov_time)
void publish(const boost::shared_ptr< M > &message) const
void kinematicsPoseMsgCallback(const thormang3_manipulation_module_msgs::KinematicsPose::ConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
#define ID_TORSO
Eigen::Quaterniond convertRotationToQuaternion(const Eigen::Matrix3d &rotation)
#define ID_L_ARM_END
#define RADIAN2DEGREE
#define ID_R_ARM_START
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
#define ID_L_ARM_START
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
void jointPoseMsgCallback(const thormang3_manipulation_module_msgs::JointPose::ConstPtr &msg)
Eigen::Matrix3d convertQuaternionToRotation(const Eigen::Quaterniond &quaternion)
thormang3_manipulation_module_msgs::KinematicsPose goal_kinematics_pose_msg_
Eigen::MatrixXd position_
bool getJointPoseCallback(thormang3_manipulation_module_msgs::GetJointPose::Request &req, thormang3_manipulation_module_msgs::GetJointPose::Response &res)
void parseIniPoseData(const std::string &path)
thormang3_manipulation_module_msgs::JointPose goal_joint_pose_msg_
#define ROS_INFO(...)
LinkData * thormang3_link_data_[ALL_JOINT_ID+1]
void setInverseKinematics(int cnt, Eigen::MatrixXd start_rotation)
void setCallbackQueue(CallbackQueueInterface *queue)
#define DEGREE2RADIAN
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void parseData(const std::string &path)
std::map< std::string, int > joint_name_to_id_
void initPoseMsgCallback(const std_msgs::String::ConstPtr &msg)
ROSLIB_DECL std::string getPath(const std::string &package_name)
bool getKinematicsPoseCallback(thormang3_manipulation_module_msgs::GetKinematicsPose::Request &req, thormang3_manipulation_module_msgs::GetKinematicsPose::Response &res)
void publishStatusMsg(unsigned int type, std::string msg)
void process(std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, double > sensors)
DynamixelState * dxl_state_
static Time now()
#define MAX_JOINT_ID
bool ok() const
void calcForwardKinematics(int joint_ID)
#define ROS_ERROR(...)
bool calcInverseKinematics(int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err)


thormang3_manipulation_module
Author(s): SCH
autogenerated on Mon Jun 10 2019 15:37:54