24 #ifndef _PLUGINS_KATANA_GOTO_OPENRAVE_THREAD_H_
25 #define _PLUGINS_KATANA_GOTO_OPENRAVE_THREAD_H_
27 #include "motion_thread.h"
30 # include <plugins/openrave/types.h>
37 class OpenRaveConnector;
48 unsigned int poll_interval_ms,
49 const std::string & robot_file,
50 const std::string & arm_model,
58 void set_target(
float x,
float y,
float z,
float phi,
float theta,
float psi);
60 set_target(
float x,
float y,
float z,
float quat_x,
float quat_y,
float quat_z,
float quat_w);
61 void set_target(
const std::string &object_name,
float rot_x);
62 void set_theta_error(
float error);
63 void set_move_straight(
bool move_straight);
64 void set_arm_extension(
bool arm_extension);
65 void set_plannerparams(std::string ¶ms,
bool straight =
false);
66 void set_plannerparams(
const char *params,
bool straight =
false);
68 virtual bool plan_target();
69 virtual void update_openrave_data();
70 virtual bool update_motor_data();
71 virtual bool move_katana();
73 static const std::string DEFAULT_PLANNERPARAMS;
74 static const std::string DEFAULT_PLANNERPARAMS_STRAIGHT;
80 std::string target_object_;
81 std::vector<std::vector<float>> * target_traj_;
82 std::vector<std::vector<float>>::iterator it_;
84 std::vector<int> motor_encoders_;
85 std::vector<float> motor_angles_;
87 const std::string cfg_robot_file_;
88 const std::string cfg_arm_model_;
89 bool cfg_autoload_IK_;
92 bool is_target_object_;
93 bool has_target_quaternion_;
95 bool is_arm_extension_;
96 std::string plannerparams_;
97 std::string plannerparams_straight_;
102 float phi_, theta_, psi_, theta_error_;
103 float quat_x_, quat_y_, quat_z_, quat_w_;
104 unsigned int poll_interval_usec_;
106 #endif //HAVE_OPENRAVE