23 #include "openrave_thread.h"
28 #include <core/threading/mutex.h>
29 #include <interfaces/JacoInterface.h>
30 #include <utils/math/angle.h>
38 # include <plugins/openrave/environment.h>
39 # include <plugins/openrave/manipulator.h>
40 # include <plugins/openrave/manipulators/kinova_jaco.h>
41 # include <plugins/openrave/robot.h>
43 using namespace OpenRAVE;
66 load_robot_ = load_robot;
68 planner_env_.env = NULL;
69 planner_env_.robot = NULL;
70 planner_env_.manip = NULL;
72 plotted_current_ =
false;
78 JacoOpenraveThread::_init()
82 cfg_manipname_ =
config->
get_string(
"/hardware/jaco/openrave/manipname/single");
86 cfg_manipname_ =
config->
get_string(
"/hardware/jaco/openrave/manipname/dual_left");
90 cfg_manipname_ =
config->
get_string(
"/hardware/jaco/openrave/manipname/dual_right");
99 JacoOpenraveThread::_load_robot()
104 cfg_OR_robot_file_ =
config->
get_string(
"/hardware/jaco/openrave/robot_file");
107 viewer_env_.robot = openrave->add_robot(cfg_OR_robot_file_,
false);
109 throw fawkes::Exception(
"Could not add robot '%s' to openrave environment. (Error: %s)",
110 cfg_OR_robot_file_.c_str(),
116 viewer_env_.manip->add_motor(0, 0);
117 viewer_env_.manip->add_motor(1, 1);
118 viewer_env_.manip->add_motor(2, 2);
119 viewer_env_.manip->add_motor(3, 3);
120 viewer_env_.manip->add_motor(4, 4);
121 viewer_env_.manip->add_motor(5, 5);
124 openrave->set_manipulator(viewer_env_.robot, viewer_env_.manip, 0.f, 0.f, 0.f);
126 openrave->get_environment()->load_IK_solver(viewer_env_.robot, OpenRAVE::IKP_Transform6D);
136 viewer_env_.robot = openrave->get_active_robot();
137 viewer_env_.manip = viewer_env_.robot->get_manipulator()->copy();
139 throw fawkes::Exception(
"%s: Could not get robot '%s' from openrave environment. (Error: %s)",
141 cfg_OR_robot_file_.c_str(),
153 JacoOpenraveThread::_post_init()
157 robot_ = viewer_env_.robot->get_robot_ptr();
161 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
162 manip_ = robot_->SetActiveManipulator(cfg_manipname_);
168 openrave->clone(planner_env_.env, planner_env_.robot, planner_env_.manip);
170 if (!planner_env_.env || !planner_env_.robot || !planner_env_.manip) {
176 case CONFIG_SINGLE: planner_env_.env->set_name(
"Planner");
break;
178 case CONFIG_LEFT: planner_env_.env->set_name(
"Planner_Left");
break;
180 case CONFIG_RIGHT: planner_env_.env->set_name(
"Planner_Right");
break;
185 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
186 RobotBase::ManipulatorPtr manip =
187 planner_env_.robot->get_robot_ptr()->SetActiveManipulator(cfg_manipname_);
188 planner_env_.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
192 robot_->GetChain(manip_->GetBase()->GetIndex(), manip_->GetEndEffector()->GetIndex(), links_);
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());
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(
389 EULER_ZXZ, x, y, z, e1, e2, e3, IKFO_IgnoreEndEffectorEnvCollisions);
399 target->coord =
false;
400 target->pos.push_back(x);
401 target->pos.push_back(y);
402 target->pos.push_back(z);
403 target->pos.push_back(e1);
404 target->pos.push_back(e2);
405 target->pos.push_back(e3);
419 solvable = planner_env_.robot->set_target_euler(
EULER_ZXZ, x, y, z, e1, e2, e3);
428 target->coord =
false;
430 planner_env_.robot->get_target().manip->get_angles_device(target->pos);
440 }
catch (openrave_exception &e) {
475 bool joints_valid =
false;
480 planner_env_.robot->get_planner_params();
490 target->coord =
false;
491 target->pos.push_back(j1);
492 target->pos.push_back(j2);
493 target->pos.push_back(j3);
494 target->pos.push_back(j4);
495 target->pos.push_back(j5);
496 target->pos.push_back(j6);
502 }
catch (openrave_exception &e) {
570 EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
571 EnvironmentMutex::scoped_lock plan_lock(planner_env_.env->get_env_ptr()->GetMutex());
572 planner_env_.robot->get_robot_ptr()->ReleaseAllGrabbed();
573 planner_env_.env->delete_all_objects();
583 viewer_env_.robot->get_robot_ptr()->GetDOFValues(dofs);
584 planner_env_.robot->get_robot_ptr()->SetDOFValues(dofs);
588 planner_env_.env->clone_objects(viewer_env_.env);
592 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
595 RobotBase::ManipulatorPtr manip =
596 planner_env_.robot->get_robot_ptr()->SetActiveManipulator(cfg_manipname_);
597 planner_env_.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
601 EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
609 vector<RobotBase::GrabbedInfoPtr> grabbed;
610 viewer_env_.robot->get_robot_ptr()->GetGrabbedInfo(grabbed);
611 for (vector<RobotBase::GrabbedInfoPtr>::iterator it = grabbed.begin(); it != grabbed.end();
614 "compare _robotlinkname '%s' with our manip link '%s'",
615 (*it)->_robotlinkname.c_str(),
616 manip->GetEndEffector()->GetName().c_str());
617 if ((*it)->_robotlinkname == manip->GetEndEffector()->GetName()) {
619 planner_env_.robot->attach_object((*it)->_grabbedname.c_str(),
621 cfg_manipname_.c_str());
630 planner_env_.robot->enable_ik_comparison(
true);
631 if (to->type == TARGET_CARTESIAN) {
632 if (!planner_env_.robot->set_target_euler(EULER_ZXZ,
649 vector<float> target;
650 planner_env_.robot->get_target().manip->get_angles(target);
651 planner_env_.robot->set_target_angles(target);
655 vector<float> target;
658 planner_env_.robot->get_target().manip->set_angles_device(to->pos);
660 planner_env_.robot->get_target().manip->get_angles(target);
661 planner_env_.robot->set_target_angles(target);
667 planner_env_.manip->set_angles_device(from->pos);
670 planner_env_.robot->set_target_plannerparams(plannerparams_);
674 planner_env_.env->run_planner(planner_env_.robot, cfg_OR_sampling_);
699 to->pos = to->trajec->back();
711 if (!cfg_OR_use_viewer_ || (!cfg_OR_plot_traj_manip_ && !cfg_OR_plot_traj_joints_))
714 graph_handle_.clear();
733 if (!target->trajec) {
739 graph_handle_.clear();
741 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
744 RobotBasePtr tmp_robot = viewer_env_.robot->get_robot_ptr();
745 RobotBase::RobotStateSaver saver(tmp_robot);
747 std::vector<dReal> joints;
759 for (jaco_trajec_t::iterator it = target->trajec->begin(); it != target->trajec->end(); ++it) {
763 tmp_robot->SetDOFValues(joints, 1, manip_->GetArmIndices());
765 if (cfg_OR_plot_traj_manip_) {
766 const OpenRAVE::Vector &trans = manip_->GetEndEffectorTransform().trans;
767 float transa[4] = {(float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w};
768 graph_handle_.push_back(viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 3.f, color_m));
771 if (cfg_OR_plot_traj_joints_) {
772 for (
unsigned int i = 0; i < links_.size(); ++i) {
773 const OpenRAVE::Vector &trans = links_[i]->GetTransform().trans;
774 float transa[4] = {(float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w};
775 graph_handle_.push_back(
776 viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 3.f, color_j));
Base Jaco Arm thread, integrating OpenRAVE.
fawkes::Mutex * planning_mutex_
mutex, used to lock when planning.
virtual void finalize()
Finalize the thread.
virtual void loop()
Mani loop.
JacoOpenraveThread(const char *name, fawkes::jaco_arm_t *arm, bool load_robot=true)
Constructor.
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.
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.
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.
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.
virtual void update_openrave()
Update the openrave environment to represent the current situation.
virtual void finalize()
Finalize the thread.
virtual void plot_first()
Plot the first target of the queue in the viewer_env.
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.
Base class for exceptions in Fawkes.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
virtual void get_joints(std::vector< float > &to) const =0
Get the joint angles of the arm.
float finger3() const
Get finger3 value.
float finger2() const
Get finger2 value.
float finger1() const
Get finger1 value.
float * joints() const
Get joints value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
void lock()
Lock this mutex.
void unlock()
Unlock the mutex.
Class containing information about all Kinova Jaco motors.
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
virtual OpenRaveManipulatorPtr copy()=0
Create a new copy of this OpenRaveManipulator instance.
void set_angles_device(std::vector< T > &angles)
Set motor angles of real device.
RefPtr<> is a reference-counting shared smartpointer.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
const char * name() const
Get name of thread.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
@ TARGET_GRIPPER
only gripper movement.
@ TARGET_CARTESIAN
target with cartesian coordinates.
@ TARGET_ANGULAR
target with angular coordinates.
@ CONFIG_SINGLE
we only have one arm.
@ CONFIG_LEFT
this arm is the left one out of two.
@ CONFIG_RIGHT
this arm is the right one out of two.
@ TRAJEC_WAITING
new trajectory target, wait for planner to process.
@ TRAJEC_READY
trajectory has been planned and is ready for execution.
@ TRAJEC_PLANNING
planner is planning the trajectory.
@ TRAJEC_PLANNING_ERROR
planner could not plan a collision-free trajectory.
@ TRAJEC_EXECUTING
trajectory is being executed.
@ TRAJEC_SKIP
skip trajectory planning for this target.
Jaco struct containing all components required for one arm.
jaco_arm_config_t config
configuration for this arm
float trajec_color[4]
the color used for plotting the trajectory.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Jaco target struct, holding information on a target.