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(),
146 #endif //HAVE_OPENRAVE 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_);
194 #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());
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_);
711 if (!cfg_OR_use_viewer_ || (!cfg_OR_plot_traj_manip_ && !cfg_OR_plot_traj_joints_))
714 graph_handle_.clear();
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));
784 #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.
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.
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
float trajec_color[4]
the color used for plotting the trajectory.
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.
virtual void get_joints(std::vector< float > &to) const =0
Get the joint angles of the arm.
target with cartesian coordinates.
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
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.
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.
fawkes::Mutex * planning_mutex_
mutex, used to lock when planning.
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.
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
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.