29 : control_cycle_sec_(0.008),
32 arm_angle_display_(false)
109 std::string _path =
ros::package::getPath(
"thormang3_manipulation_module") +
"/config/ik_weight.yaml";
119 doc = YAML::LoadFile(path.c_str());
120 }
catch (
const std::exception& e)
126 YAML::Node ik_weight_node = doc[
"weight_value"];
127 for (YAML::iterator it = ik_weight_node.begin(); it != ik_weight_node.end(); ++it)
129 int id = it->first.as<
int>();
130 double value = it->second.as<
double>();
142 doc = YAML::LoadFile(path.c_str());
143 }
catch (
const std::exception& e)
150 double mov_time = doc[
"mov_time"].as<
double>();
154 YAML::Node tar_pose_node = doc[
"tar_pose"];
155 for (YAML::iterator it = tar_pose_node.begin(); it != tar_pose_node.end(); ++it)
157 int id = it->first.as<
int>();
158 double value = it->second.as<
double>();
200 if (msg->data ==
"ini_pose")
203 std::string ini_pose_path =
ros::package::getPath(
"thormang3_manipulation_module") +
"/config/ini_pose.yaml";
221 thormang3_manipulation_module_msgs::GetJointPose::Response &res)
226 for (
int name_index = 1; name_index <=
MAX_JOINT_ID; name_index++)
239 thormang3_manipulation_module_msgs::GetKinematicsPose::Response &res)
246 if (req.group_name ==
"left_arm")
248 else if (req.group_name ==
"right_arm")
250 else if (req.group_name ==
"left_arm_with_torso")
252 else if (req.group_name ==
"right_arm_with_torso")
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();
350 ROS_INFO(
"[start] send trajectory");
359 double mov_time = 2.0;
365 double diff = fabs(tar_value - ini_value);
402 ROS_INFO(
"[start] send trajectory");
411 double mov_time = 2.0;
435 for (
int dim = 0; dim < 3; dim++)
457 ROS_INFO(
"[start] send trajectory");
462 for (
int dim = 0; dim < 3; dim++)
474 Eigen::Quaterniond _quaternion = start_quaternion.slerp(count, target_quaternion);
480 std::map<std::string, double> sensors)
487 for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter =
result_.begin();
488 state_iter !=
result_.end(); state_iter++)
490 std::string joint_name = state_iter->first;
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;
518 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO,
"Start Trajectory");
529 double ik_tol = 1e-3;
537 if (ik_success ==
true)
547 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO,
"IK Failed");
568 for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter =
result_.begin();
569 state_iter !=
result_.end(); state_iter++)
571 std::string joint_name = state_iter->first;
582 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO,
"End Trajectory");
630 robotis_controller_msgs::StatusMsg status;
633 status.module_name =
"Manipulation";
634 status.status_msg = msg;
Eigen::VectorXd present_joint_position_
Eigen::MatrixXd goal_joint_tra_
Eigen::MatrixXd orientation_
boost::thread * traj_generate_tread_
std::map< std::string, DynamixelState * > result_
double control_cycle_sec_
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
Eigen::MatrixXd ik_start_rotation_
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())
ros::Publisher status_msg_pub_
Eigen::Quaterniond convertRotationToQuaternion(const Eigen::Matrix3d &rotation)
void taskTrajGenerateProc()
void jointTrajGenerateProc()
Eigen::VectorXd goal_joint_position_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ControlMode control_mode_
ros::Publisher movement_done_pub_
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_
LinkData * thormang3_link_data_[ALL_JOINT_ID+1]
Eigen::MatrixXd ik_target_rotation_
void setInverseKinematics(int cnt, Eigen::MatrixXd start_rotation)
void setCallbackQueue(CallbackQueueInterface *queue)
KinematicsDynamics * robotis_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void parseData(const std::string &path)
Eigen::MatrixXd ik_target_position_
std::map< std::string, int > joint_name_to_id_
void initPoseMsgCallback(const std_msgs::String::ConstPtr &msg)
Eigen::MatrixXd ik_weight_
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_
std_msgs::String movement_done_msg_
Eigen::MatrixXd goal_task_tra_
void calcForwardKinematics(int joint_ID)
bool calcInverseKinematics(int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err)
Eigen::VectorXd init_joint_position_
void initPoseTrajGenerateProc()
virtual ~ManipulationModule()
boost::thread queue_thread_