Public Member Functions | Public Attributes | Private Member Functions | Private Attributes | List of all members
thormang3::ManipulationModule Class Reference

#include <manipulation_module.h>

Inheritance diagram for thormang3::ManipulationModule:
Inheritance graph
[legend]

Public Member Functions

bool getJointPoseCallback (thormang3_manipulation_module_msgs::GetJointPose::Request &req, thormang3_manipulation_module_msgs::GetJointPose::Response &res)
 
bool getKinematicsPoseCallback (thormang3_manipulation_module_msgs::GetKinematicsPose::Request &req, thormang3_manipulation_module_msgs::GetKinematicsPose::Response &res)
 
void initialize (const int control_cycle_msec, robotis_framework::Robot *robot)
 
void initPoseMsgCallback (const std_msgs::String::ConstPtr &msg)
 
void initPoseTrajGenerateProc ()
 
bool isRunning ()
 
void jointPoseMsgCallback (const thormang3_manipulation_module_msgs::JointPose::ConstPtr &msg)
 
void jointTrajGenerateProc ()
 
void kinematicsPoseMsgCallback (const thormang3_manipulation_module_msgs::KinematicsPose::ConstPtr &msg)
 
 ManipulationModule ()
 
void process (std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, double > sensors)
 
void publishStatusMsg (unsigned int type, std::string msg)
 
void stop ()
 
void taskTrajGenerateProc ()
 
virtual ~ManipulationModule ()
 
- Public Member Functions inherited from robotis_framework::MotionModule
ControlMode getControlMode ()
 
bool getModuleEnable ()
 
std::string getModuleName ()
 
virtual void onModuleDisable ()
 
virtual void onModuleEnable ()
 
void setModuleEnable (bool enable)
 
virtual ~MotionModule ()
 

Public Attributes

KinematicsDynamicsrobotis_
 
- Public Attributes inherited from robotis_framework::MotionModule
std::map< std::string, DynamixelState * > result_
 

Private Member Functions

void parseData (const std::string &path)
 
void parseIniPoseData (const std::string &path)
 
void queueThread ()
 
void setInverseKinematics (int cnt, Eigen::MatrixXd start_rotation)
 

Private Attributes

int all_time_steps_
 
bool arm_angle_display_
 
int cnt_
 
double control_cycle_sec_
 
thormang3_manipulation_module_msgs::JointPose goal_joint_pose_msg_
 
Eigen::VectorXd goal_joint_position_
 
Eigen::MatrixXd goal_joint_tra_
 
thormang3_manipulation_module_msgs::KinematicsPose goal_kinematics_pose_msg_
 
Eigen::MatrixXd goal_task_tra_
 
int ik_id_end_
 
int ik_id_start_
 
bool ik_solving_
 
Eigen::MatrixXd ik_start_rotation_
 
Eigen::MatrixXd ik_target_position_
 
Eigen::MatrixXd ik_target_rotation_
 
Eigen::MatrixXd ik_weight_
 
Eigen::VectorXd init_joint_position_
 
bool is_moving_
 
std::map< std::string, int > joint_name_to_id_
 
double mov_time_
 
std_msgs::String movement_done_msg_
 
ros::Publisher movement_done_pub_
 
Eigen::VectorXd present_joint_position_
 
boost::thread queue_thread_
 
ros::Publisher status_msg_pub_
 
boost::thread * traj_generate_tread_
 

Additional Inherited Members

- Static Public Member Functions inherited from robotis_framework::Singleton< ManipulationModule >
static void destroyInstance ()
 
static T * getInstance ()
 
- Protected Member Functions inherited from robotis_framework::Singleton< ManipulationModule >
Singletonoperator= (Singleton const &)
 
 Singleton (Singleton const &)
 
 Singleton ()
 
- Protected Attributes inherited from robotis_framework::MotionModule
ControlMode control_mode_
 
bool enable_
 
std::string module_name_
 

Detailed Description

Definition at line 56 of file manipulation_module.h.

Constructor & Destructor Documentation

ManipulationModule::ManipulationModule ( )

Definition at line 28 of file manipulation_module.cpp.

ManipulationModule::~ManipulationModule ( )
virtual

Definition at line 91 of file manipulation_module.cpp.

Member Function Documentation

bool ManipulationModule::getJointPoseCallback ( thormang3_manipulation_module_msgs::GetJointPose::Request &  req,
thormang3_manipulation_module_msgs::GetJointPose::Response &  res 
)

Definition at line 220 of file manipulation_module.cpp.

bool ManipulationModule::getKinematicsPoseCallback ( thormang3_manipulation_module_msgs::GetKinematicsPose::Request &  req,
thormang3_manipulation_module_msgs::GetKinematicsPose::Response &  res 
)

Definition at line 238 of file manipulation_module.cpp.

void ManipulationModule::initialize ( const int  control_cycle_msec,
robotis_framework::Robot robot 
)
virtual

Implements robotis_framework::MotionModule.

Definition at line 96 of file manipulation_module.cpp.

void ManipulationModule::initPoseMsgCallback ( const std_msgs::String::ConstPtr &  msg)

Definition at line 193 of file manipulation_module.cpp.

void ManipulationModule::initPoseTrajGenerateProc ( )

Definition at line 332 of file manipulation_module.cpp.

bool ManipulationModule::isRunning ( )
virtual

Implements robotis_framework::MotionModule.

Definition at line 623 of file manipulation_module.cpp.

void ManipulationModule::jointPoseMsgCallback ( const thormang3_manipulation_module_msgs::JointPose::ConstPtr &  msg)

Definition at line 314 of file manipulation_module.cpp.

void ManipulationModule::jointTrajGenerateProc ( )

Definition at line 353 of file manipulation_module.cpp.

void ManipulationModule::kinematicsPoseMsgCallback ( const thormang3_manipulation_module_msgs::KinematicsPose::ConstPtr &  msg)

Definition at line 271 of file manipulation_module.cpp.

void ManipulationModule::parseData ( const std::string &  path)
private

Definition at line 113 of file manipulation_module.cpp.

void ManipulationModule::parseIniPoseData ( const std::string &  path)
private

Definition at line 136 of file manipulation_module.cpp.

void ManipulationModule::process ( std::map< std::string, robotis_framework::Dynamixel * >  dxls,
std::map< std::string, double >  sensors 
)
virtual

Implements robotis_framework::MotionModule.

Definition at line 479 of file manipulation_module.cpp.

void ManipulationModule::publishStatusMsg ( unsigned int  type,
std::string  msg 
)

Definition at line 628 of file manipulation_module.cpp.

void ManipulationModule::queueThread ( )
private

Definition at line 167 of file manipulation_module.cpp.

void ManipulationModule::setInverseKinematics ( int  cnt,
Eigen::MatrixXd  start_rotation 
)
private

Definition at line 460 of file manipulation_module.cpp.

void ManipulationModule::stop ( )
virtual

Implements robotis_framework::MotionModule.

Definition at line 614 of file manipulation_module.cpp.

void ManipulationModule::taskTrajGenerateProc ( )

Definition at line 405 of file manipulation_module.cpp.

Member Data Documentation

int thormang3::ManipulationModule::all_time_steps_
private

Definition at line 115 of file manipulation_module.h.

bool thormang3::ManipulationModule::arm_angle_display_
private

Definition at line 95 of file manipulation_module.h.

int thormang3::ManipulationModule::cnt_
private

Definition at line 114 of file manipulation_module.h.

double thormang3::ManipulationModule::control_cycle_sec_
private

Definition at line 97 of file manipulation_module.h.

thormang3_manipulation_module_msgs::JointPose thormang3::ManipulationModule::goal_joint_pose_msg_
private

Definition at line 121 of file manipulation_module.h.

Eigen::VectorXd thormang3::ManipulationModule::goal_joint_position_
private

Definition at line 108 of file manipulation_module.h.

Eigen::MatrixXd thormang3::ManipulationModule::goal_joint_tra_
private

Definition at line 117 of file manipulation_module.h.

thormang3_manipulation_module_msgs::KinematicsPose thormang3::ManipulationModule::goal_kinematics_pose_msg_
private

Definition at line 122 of file manipulation_module.h.

Eigen::MatrixXd thormang3::ManipulationModule::goal_task_tra_
private

Definition at line 118 of file manipulation_module.h.

int thormang3::ManipulationModule::ik_id_end_
private

Definition at line 127 of file manipulation_module.h.

int thormang3::ManipulationModule::ik_id_start_
private

Definition at line 126 of file manipulation_module.h.

bool thormang3::ManipulationModule::ik_solving_
private

Definition at line 125 of file manipulation_module.h.

Eigen::MatrixXd thormang3::ManipulationModule::ik_start_rotation_
private

Definition at line 130 of file manipulation_module.h.

Eigen::MatrixXd thormang3::ManipulationModule::ik_target_position_
private

Definition at line 129 of file manipulation_module.h.

Eigen::MatrixXd thormang3::ManipulationModule::ik_target_rotation_
private

Definition at line 131 of file manipulation_module.h.

Eigen::MatrixXd thormang3::ManipulationModule::ik_weight_
private

Definition at line 132 of file manipulation_module.h.

Eigen::VectorXd thormang3::ManipulationModule::init_joint_position_
private

Definition at line 109 of file manipulation_module.h.

bool thormang3::ManipulationModule::is_moving_
private

Definition at line 112 of file manipulation_module.h.

std::map<std::string, int> thormang3::ManipulationModule::joint_name_to_id_
private

Definition at line 136 of file manipulation_module.h.

double thormang3::ManipulationModule::mov_time_
private

Definition at line 113 of file manipulation_module.h.

std_msgs::String thormang3::ManipulationModule::movement_done_msg_
private

Definition at line 101 of file manipulation_module.h.

ros::Publisher thormang3::ManipulationModule::movement_done_pub_
private

Definition at line 104 of file manipulation_module.h.

Eigen::VectorXd thormang3::ManipulationModule::present_joint_position_
private

Definition at line 107 of file manipulation_module.h.

boost::thread thormang3::ManipulationModule::queue_thread_
private

Definition at line 98 of file manipulation_module.h.

KinematicsDynamics* thormang3::ManipulationModule::robotis_

Definition at line 87 of file manipulation_module.h.

ros::Publisher thormang3::ManipulationModule::status_msg_pub_
private

Definition at line 103 of file manipulation_module.h.

boost::thread* thormang3::ManipulationModule::traj_generate_tread_
private

Definition at line 99 of file manipulation_module.h.


The documentation for this class was generated from the following files:


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