23 #include "openrave_thread.h" 27 #include <interfaces/JacoInterface.h> 28 #include <core/threading/mutex.h> 29 #include <utils/math/angle.h> 37 #include <plugins/openrave/environment.h> 38 #include <plugins/openrave/robot.h> 39 #include <plugins/openrave/manipulator.h> 40 #include <plugins/openrave/manipulators/kinova_jaco.h> 65 __load_robot = load_robot;
67 __planner_env.env = NULL;
68 __planner_env.robot = NULL;
69 __planner_env.manip = NULL;
71 __plotted_current =
false;
77 JacoOpenraveThread::_init()
81 __cfg_manipname =
config->
get_string(
"/hardware/jaco/openrave/manipname/single");
85 __cfg_manipname =
config->
get_string(
"/hardware/jaco/openrave/manipname/dual_left");
89 __cfg_manipname =
config->
get_string(
"/hardware/jaco/openrave/manipname/dual_right");
100 JacoOpenraveThread::_load_robot()
105 __cfg_OR_robot_file =
config->
get_string(
"/hardware/jaco/openrave/robot_file");
108 __viewer_env.robot = openrave->add_robot(__cfg_OR_robot_file,
false);
115 __viewer_env.manip->add_motor(0,0);
116 __viewer_env.manip->add_motor(1,1);
117 __viewer_env.manip->add_motor(2,2);
118 __viewer_env.manip->add_motor(3,3);
119 __viewer_env.manip->add_motor(4,4);
120 __viewer_env.manip->add_motor(5,5);
123 openrave->set_manipulator(__viewer_env.robot, __viewer_env.manip, 0.f, 0.f, 0.f);
125 openrave->get_environment()->load_IK_solver(__viewer_env.robot, OpenRAVE::IKP_Transform6D);
135 __viewer_env.robot = openrave->get_active_robot();
136 __viewer_env.manip = __viewer_env.robot->get_manipulator()->copy();
142 #endif //HAVE_OPENRAVE 149 JacoOpenraveThread::_post_init()
153 __robot = __viewer_env.robot->get_robot_ptr();
157 EnvironmentMutex::scoped_lock lock(__viewer_env.env->get_env_ptr()->GetMutex());
158 __manip = __robot->SetActiveManipulator(__cfg_manipname);
164 openrave->clone(__planner_env.env, __planner_env.robot, __planner_env.manip);
166 if( !__planner_env.env || !__planner_env.robot || !__planner_env.manip) {
173 __planner_env.env->set_name(
"Planner");
177 __planner_env.env->set_name(
"Planner_Left");
181 __planner_env.env->set_name(
"Planner_Right");
187 EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex());
188 RobotBase::ManipulatorPtr manip = __planner_env.robot->get_robot_ptr()->SetActiveManipulator(__cfg_manipname);
189 __planner_env.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
193 __robot->GetChain( __manip->GetBase()->GetIndex(), __manip->GetEndEffector()->GetIndex(), __links);
195 #endif //HAVE_OPENRAVE 203 openrave->set_active_robot( NULL );
205 __planner_env.robot = NULL;
206 __planner_env.manip = NULL;
207 __planner_env.env = NULL;
226 #ifndef HAVE_OPENRAVE 229 if( __arm == NULL || __arm->
arm == NULL ) {
238 jaco_target_queue_t::iterator it;
256 from->
pos = (*it)->trajec->back();
278 _plan_path(from, to);
292 #ifndef HAVE_OPENRAVE 295 if( __arm == NULL || __arm->
iface == NULL || __robot == NULL || __manip == NULL )
308 __viewer_env.manip->set_angles_device(__joints);
309 __viewer_env.manip->get_angles(__joints);
312 EnvironmentMutex::scoped_lock lock(__viewer_env.env->get_env_ptr()->GetMutex());
313 __robot->SetDOFValues(__joints, 1, __manip->GetArmIndices());
321 EnvironmentMutex::scoped_lock lock(__viewer_env.env->get_env_ptr()->GetMutex());
322 __robot->SetDOFValues(__joints, 1, __manip->GetGripperIndices());
325 if( __plot_current ) {
326 EnvironmentMutex::scoped_lock lock(__viewer_env.env->get_env_ptr()->GetMutex());
328 if( !__plotted_current ) {
330 __graph_current.clear();
333 if( __cfg_OR_plot_cur_manip ) {
334 OpenRAVE::Vector color(0.f, 1.f, 0.f, 1.f);
335 const OpenRAVE::Vector &trans = __manip->GetEndEffectorTransform().trans;
336 float transa[4] = { (float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w };
337 __graph_current.push_back( __viewer_env.env->get_env_ptr()->plot3(transa,1, 0, 2.f, color));
340 if( __cfg_OR_plot_cur_joints ) {
341 OpenRAVE::Vector color(0.2f, 1.f, 0.f, 1.f);
342 for(
unsigned int i=0; i<__links.size(); ++i) {
343 const OpenRAVE::Vector &trans = __links[i]->GetTransform().trans;
344 float transa[4] = { (float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w };
345 __graph_current.push_back( __viewer_env.env->get_env_ptr()->plot3(transa,1, 0, 2.f, color));
349 __plotted_current = __plot_current;
351 }
catch( openrave_exception &e) {
377 bool solvable =
false;
382 __planner_env.robot->get_planner_params();
387 __planner_env.robot->enable_ik_comparison(
false);
388 solvable = __planner_env.robot->set_target_euler(
EULER_ZXZ, x, y, z, e1, e2, e3, IKFO_IgnoreEndEffectorEnvCollisions);
399 target->
pos.push_back(x);
400 target->
pos.push_back(y);
401 target->
pos.push_back(z);
402 target->
pos.push_back(e1);
403 target->
pos.push_back(e2);
404 target->
pos.push_back(e3);
418 solvable = __planner_env.robot->set_target_euler(
EULER_ZXZ, x, y, z, e1, e2, e3);
429 __planner_env.robot->get_target().manip->get_angles_device(target->
pos);
439 }
catch( openrave_exception &e) {
469 bool joints_valid =
false;
474 __planner_env.robot->get_planner_params();
485 target->
pos.push_back(j1);
486 target->
pos.push_back(j2);
487 target->
pos.push_back(j3);
488 target->
pos.push_back(j4);
489 target->
pos.push_back(j5);
490 target->
pos.push_back(j6);
496 }
catch( openrave_exception &e) {
561 EnvironmentMutex::scoped_lock view_lock(__viewer_env.env->get_env_ptr()->GetMutex());
562 EnvironmentMutex::scoped_lock plan_lock(__planner_env.env->get_env_ptr()->GetMutex());
563 __planner_env.robot->get_robot_ptr()->ReleaseAllGrabbed();
564 __planner_env.env->delete_all_objects();
574 __viewer_env.robot->get_robot_ptr()->GetDOFValues(dofs);
575 __planner_env.robot->get_robot_ptr()->SetDOFValues(dofs);
579 __planner_env.env->clone_objects( __viewer_env.env );
583 EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex());
586 RobotBase::ManipulatorPtr manip = __planner_env.robot->get_robot_ptr()->SetActiveManipulator(__cfg_manipname);
587 __planner_env.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
591 EnvironmentMutex::scoped_lock view_lock(__viewer_env.env->get_env_ptr()->GetMutex());
599 vector<RobotBase::GrabbedInfoPtr> grabbed;
600 __viewer_env.robot->get_robot_ptr()->GetGrabbedInfo(grabbed);
601 for( vector<RobotBase::GrabbedInfoPtr>::iterator it=grabbed.begin(); it!=grabbed.end(); ++it ) {
603 (*it)->_robotlinkname.c_str(), manip->GetEndEffector()->GetName().c_str());
604 if( (*it)->_robotlinkname == manip->GetEndEffector()->GetName() ) {
606 __planner_env.robot->attach_object((*it)->_grabbedname.c_str(), __planner_env.env, __cfg_manipname.c_str());
615 __planner_env.robot->enable_ik_comparison(
true);
617 if( !__planner_env.robot->set_target_euler(
EULER_ZXZ, to->
pos.at(0), to->
pos.at(1), to->
pos.at(2), to->
pos.at(3), to->
pos.at(4), to->
pos.at(5)) ) {
628 vector<float> target;
629 __planner_env.robot->get_target().manip->get_angles(target);
630 __planner_env.robot->set_target_angles(target);
634 vector<float> target;
637 __planner_env.robot->get_target().manip->set_angles_device(to->
pos);
639 __planner_env.robot->get_target().manip->get_angles(target);
640 __planner_env.robot->set_target_angles(target);
646 __planner_env.manip->set_angles_device(from->
pos);
649 __planner_env.robot->set_target_plannerparams(__plannerparams);
653 __planner_env.env->run_planner(__planner_env.robot, __cfg_OR_sampling);
692 if( !__cfg_OR_use_viewer || (!__cfg_OR_plot_traj_manip && !__cfg_OR_plot_traj_joints))
695 __graph_handle.clear();
721 __graph_handle.clear();
723 EnvironmentMutex::scoped_lock lock(__viewer_env.env->get_env_ptr()->GetMutex());
726 RobotBasePtr tmp_robot = __viewer_env.robot->get_robot_ptr();
727 RobotBase::RobotStateSaver saver(tmp_robot);
729 std::vector<dReal> joints;
741 for(jaco_trajec_t::iterator it = target->
trajec->begin(); it!=target->
trajec->end(); ++it) {
745 tmp_robot->SetDOFValues(joints, 1, __manip->GetArmIndices());
747 if( __cfg_OR_plot_traj_manip ) {
748 const OpenRAVE::Vector &trans = __manip->GetEndEffectorTransform().trans;
749 float transa[4] = { (float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w };
750 __graph_handle.push_back( __viewer_env.env->get_env_ptr()->plot3(transa,1, 0, 3.f, color_m));
753 if( __cfg_OR_plot_traj_joints ) {
754 for(
unsigned int i=0; i<__links.size(); ++i) {
755 const OpenRAVE::Vector &trans = __links[i]->GetTransform().trans;
756 float transa[4] = { (float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w };
757 __graph_handle.push_back( __viewer_env.env->get_env_ptr()->plot3(transa,1, 0, 3.f, color_j));
765 #endif //HAVE_OPENRAVE trajectory has been planned and is ready for execution.
void set_angles_device(std::vector< T > &angles)
Set motor angles of real device.
virtual OpenRaveManipulatorPtr copy()=0
Create a new copy of this OpenRaveManipulator instance.
Jaco struct containing all components required for one arm.
fawkes::Mutex * __planning_mutex
mutex, used to lock when planning.
virtual void update_openrave()
Update the openrave environment to represent the current situation.
Fawkes library namespace.
void unlock()
Unlock the mutex.
virtual void plot_first()
Plot the first target of the queue in the viewer_env.
fawkes::RefPtr< jaco_trajec_t > trajec
trajectory, if target is TARGET_TRAJEC.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
float finger2() const
Get finger2 value.
float trajec_color[4]
the color used for plotting the trajectory.
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
trajectory is being executed.
Logger * logger
This is the Logger member used to access the logger.
jaco_target_type_t type
target type.
planner could not plan a collision-free trajectory.
jaco_trajec_point_t pos
target position (interpreted depending on target type).
virtual void finalize()
Finalize the thread.
jaco_trajec_state_t trajec_state
state of the trajectory, if target is TARGET_TRAJEC.
virtual bool set_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true)
Flush the target_queue and add this one.
this arm is the right one out of two.
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
JacoOpenraveThread(const char *name, fawkes::jaco_arm_t *arm, bool load_robot=true)
Constructor.
virtual bool add_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true)
Add target joint values to the queue.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
float finger1() const
Get finger1 value.
Class containing information about all Kinova Jaco motors.
float * joints() const
Get joints value.
Base class for exceptions in Fawkes.
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
virtual void get_joints(std::vector< float > &to) const =0
Get the joint angles of the arm.
target with cartesian coordinates.
Jaco target struct, holding information on a target.
target with angular coordinates.
const char * name() const
Get name of thread.
jaco_arm_config_t config
configuration for this arm
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
planner is planning the trajectory.
bool coord
this target needs to be coordinated with targets of other arms.
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
this arm is the left one out of two.
RefPtr<> is a reference-counting shared smartpointer.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void finalize()
Finalize the thread.
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
skip trajectory planning for this target.
new trajectory target, wait for planner to process.
void lock()
Lock this mutex.
virtual bool add_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true)
Solve IK and add target to the queue.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
virtual void loop()
Mani loop.
float finger3() const
Get finger3 value.
Configuration * config
This is the Configuration member used to access the configuration.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual bool set_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true)
Flush the target_queue and add this one.
Base Jaco Arm thread, integrating OpenRAVE.