Changes have been pushed for the project "Fawkes Robotics Software Framework".
Gitweb: http://git.fawkesrobotics.org/fawkes.git Trac: http://trac.fawkesrobotics.org The branch, bahram/kinova_dualarm has been updated to 104f80541e25967434d1b838605dd99325b643cc (commit) via 2cdfa8621b084a6a9ee4fa1c5d4674abcf779dcc (commit) via 890173238910bec722e682cb3207f4aedc829ec8 (commit) via b2fb2d67ca82196b48826acac7907d47eeba4d81 (commit) from bbfe6dca6d05bbf1dd70301e73b15af01b95d63c (commit) http://git.fawkesrobotics.org/fawkes.git/bahram/kinova_dualarm Those revisions listed above that are new to this repository have not appeared on any other notification email; so we list those revisions in full, below. - *Log* --------------------------------------------------------------- commit b2fb2d67ca82196b48826acac7907d47eeba4d81 Author: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> AuthorDate: Wed Aug 6 23:25:40 2014 +0200 Commit: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> CommitDate: Wed Aug 6 23:25:40 2014 +0200 openrave: fix setting target by joint_angles The method was just for debugging and marked as "temporary". It is risky to use that method, as it skips any checks (even IK check). It can come in handy though, in case we check for IK solvability and want to store the resultet joint configuration for later. http://git.fawkesrobotics.org/fawkes.git/commit/b2fb2d6 http://trac.fawkesrobotics.org/changeset/b2fb2d6 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 890173238910bec722e682cb3207f4aedc829ec8 Author: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> AuthorDate: Wed Aug 6 23:35:02 2014 +0200 Commit: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> CommitDate: Wed Aug 6 23:35:02 2014 +0200 kinova: openrave_thread: add FIFO queue for targets and trajectories The target_queue is (not yet) being processed FIFO, running path-planner for each target. If successful, the resultet trajectory is added to the trajec_queue (FIFO) which is accessed by the goto_thread to check if a new trajectory is to be processed. Adding a target to target_queue automatically checks for IK solvability (which is the boolean return value). The target is only enqueued if solvable, and if the parameter "plan" is not explicitly set to false. Skipping planning means here that the IK-solved target is added as a single trajectory-point to the trajec_queue. This can be dangerous, especially for collision avoidance, but can also be handy if the user knows what he is doing (e.g. very short distances with the Kinova arm can be done without planning, the internal controllers handle those straight movements pretty well). http://git.fawkesrobotics.org/fawkes.git/commit/8901732 http://trac.fawkesrobotics.org/changeset/8901732 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 2cdfa8621b084a6a9ee4fa1c5d4674abcf779dcc Author: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> AuthorDate: Thu Aug 7 00:55:14 2014 +0200 Commit: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> CommitDate: Thu Aug 7 00:55:14 2014 +0200 openrave: can set plannerparams by passing const char* now http://git.fawkesrobotics.org/fawkes.git/commit/2cdfa86 http://trac.fawkesrobotics.org/changeset/2cdfa86 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 104f80541e25967434d1b838605dd99325b643cc Author: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> AuthorDate: Fri Aug 8 11:33:46 2014 +0200 Commit: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> CommitDate: Fri Aug 8 11:33:46 2014 +0200 kinova: add planning for single arm Each openrave_thread starts planning as soon as there is a target in the target_queue. Upon success, the trajectory is added to the trajec_queue, which is repeatedly checked by the goto_thread, in order to execute the trajectory. Proper trajectory execution needs to be added though. The planning is probably not safe, because currently it all happens in the same openrave environment. We need clone-environments for each planner, especially when using multiple arms. http://git.fawkesrobotics.org/fawkes.git/commit/104f805 http://trac.fawkesrobotics.org/changeset/104f805 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *Summary* ----------------------------------------------------------- src/plugins/kinova/act_thread.cpp | 8 +- src/plugins/kinova/goto_thread.cpp | 24 ++++- src/plugins/kinova/openrave_base_thread.cpp | 48 +++++++ src/plugins/kinova/openrave_base_thread.h | 16 +++- src/plugins/kinova/openrave_dual_thread.cpp | 22 +++- src/plugins/kinova/openrave_dual_thread.h | 3 +- src/plugins/kinova/openrave_single_thread.cpp | 164 ++++++++++++++++++++----- src/plugins/kinova/openrave_single_thread.h | 6 +- src/plugins/openrave/robot.cpp | 12 ++- src/plugins/openrave/robot.h | 9 +- 10 files changed, 262 insertions(+), 50 deletions(-) - *Diffs* ------------------------------------------------------------- - *commit* b2fb2d67ca82196b48826acac7907d47eeba4d81 - - - - - - - - - - Author: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> Date: Wed Aug 6 23:25:40 2014 +0200 Subject: openrave: fix setting target by joint_angles src/plugins/openrave/robot.cpp | 3 ++- src/plugins/openrave/robot.h | 8 ++++---- 2 files changed, 6 insertions(+), 5 deletions(-) _Diff for modified files_: diff --git a/src/plugins/openrave/robot.cpp b/src/plugins/openrave/robot.cpp index 8056b67..ed3c05e 100644 --- a/src/plugins/openrave/robot.cpp +++ b/src/plugins/openrave/robot.cpp @@ -468,7 +468,6 @@ OpenRaveRobot::set_target_plannerparams(std::string& params) __target.plannerparams = params; } -// just temporary! no IK check etc involved /** Set target angles directly. * @param angles vector with angle values */ @@ -476,6 +475,8 @@ void OpenRaveRobot::set_target_angles( std::vector<float>& angles ) { __target.manip->set_angles(angles); + __target.type = TARGET_JOINTS; + __target.solvable = true; //no IK check done though! } diff --git a/src/plugins/openrave/robot.h b/src/plugins/openrave/robot.h index 8e0b2cd..986d5af 100644 --- a/src/plugins/openrave/robot.h +++ b/src/plugins/openrave/robot.h @@ -67,7 +67,7 @@ class OpenRaveRobot virtual bool set_target_euler(euler_rotation_t type, float trans_x, float trans_y, float trans_z, float phi, float theta, float psi, bool no_offset = false); virtual bool set_target_ikparam(OpenRAVE::IkParameterization ik_param); virtual void set_target_plannerparams(std::string& params); - virtual void set_target_angles( std::vector<float>& angles ); // just temporary + virtual void set_target_angles( std::vector<float>& angles ); virtual bool set_target_object_position(float trans_x, float trans_y, float trans_z, float rot_x); @@ -88,12 +88,12 @@ class OpenRaveRobot bool set_target_euler(OpenRAVE::Vector& trans, std::vector<float>& rotations, bool no_offset = false); OpenRAVE::IkParameterization get_5dof_ikparam(OpenRAVE::Transform& trans); - fawkes::Logger* __logger; + fawkes::Logger* __logger; std::string __name; OpenRAVE::RobotBasePtr __robot; OpenRAVE::RobotBase::ManipulatorPtr __arm; - OpenRaveManipulator* __manip; + OpenRaveManipulator* __manip; target_t __target; @@ -111,4 +111,4 @@ class OpenRaveRobot } // end of namespace fawkes -#endif \ No newline at end of file +#endif - *commit* 890173238910bec722e682cb3207f4aedc829ec8 - - - - - - - - - - Author: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> Date: Wed Aug 6 23:35:02 2014 +0200 Subject: kinova: openrave_thread: add FIFO queue for targets and trajectories src/plugins/kinova/act_thread.cpp | 8 ++-- src/plugins/kinova/goto_thread.cpp | 24 +++++++++- src/plugins/kinova/openrave_base_thread.cpp | 35 +++++++++++++ src/plugins/kinova/openrave_base_thread.h | 15 +++++- src/plugins/kinova/openrave_dual_thread.cpp | 22 +++++++-- src/plugins/kinova/openrave_dual_thread.h | 3 +- src/plugins/kinova/openrave_single_thread.cpp | 66 +++++++++++++++++++------ src/plugins/kinova/openrave_single_thread.h | 3 +- 8 files changed, 148 insertions(+), 28 deletions(-) _Diff for modified files_: diff --git a/src/plugins/kinova/act_thread.cpp b/src/plugins/kinova/act_thread.cpp index a88e522..acdb9db 100644 --- a/src/plugins/kinova/act_thread.cpp +++ b/src/plugins/kinova/act_thread.cpp @@ -432,12 +432,12 @@ KinovaActThread::_process_msgs_arm(jaco_arm_t &arm) msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3()); #ifdef HAVE_OPENRAVE logger->log_debug(name(), "%s: CartesianGotoMessage is being passed to openrave", arm.iface->id()); - std::vector<float> v = arm.openrave_thread->set_target(msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3()); - if( v.size() == 6 ) - arm.goto_thread->set_target_ang(v.at(0), v.at(1), v.at(2), v.at(3), v.at(4), v.at(5)); - else + // add target to OpenRAVE queue, but skip planning for now (not implemented yet) + bool solvable = arm.openrave_thread->add_target(msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3(), false); + if( !solvable ) { logger->log_warn(name(), "Failed executing CartesianGotoMessage, arm %s and/or thread %s could not find IK solution", arm.arm->get_name().c_str(), arm.openrave_thread->name()); + } #else arm.goto_thread->set_target(msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3()); #endif diff --git a/src/plugins/kinova/goto_thread.cpp b/src/plugins/kinova/goto_thread.cpp index 6af3e49..56e995b 100644 --- a/src/plugins/kinova/goto_thread.cpp +++ b/src/plugins/kinova/goto_thread.cpp @@ -21,6 +21,7 @@ */ #include "goto_thread.h" +#include "openrave_base_thread.h" #include "arm.h" #include <interfaces/JacoInterface.h> @@ -312,7 +313,28 @@ KinovaGotoThread::loop() } else if( !__final ) { // check for final position check_final(); - } + + } else { + // all current trajecs have been processed. check for new + if( __arm->openrave_thread->trajec_ready() ) { + //logger->log_debug(name(), "new trajectory ready! processing now..."); + std::vector< std::vector<float> >* trajec = __arm->openrave_thread->pop_trajec(); + + if( trajec != NULL ) { + if( !trajec->empty() ) { + //logger->log_debug(name(), "...setting new target now."); + // TODO: no planning yet, so use last trajec point + std::vector<float> target = trajec->back(); + set_target_ang(target.at(0), target.at(1), target.at(2), + target.at(3), target.at(4), target.at(5)); + } + + // delete the trajectory, it is not needed anywhere anymore + delete trajec; + trajec = NULL; + } + } + } __arm->iface->set_final(__final); } diff --git a/src/plugins/kinova/openrave_base_thread.cpp b/src/plugins/kinova/openrave_base_thread.cpp index 2de378f..82b8d7a 100644 --- a/src/plugins/kinova/openrave_base_thread.cpp +++ b/src/plugins/kinova/openrave_base_thread.cpp @@ -23,6 +23,7 @@ #include "openrave_base_thread.h" #include <interfaces/JacoInterface.h> +#include <core/threading/mutex.h> #include <cmath> #include <stdio.h> @@ -37,6 +38,7 @@ #endif using namespace fawkes; +using namespace std; /** @class KinovaOpenraveBaseThread "openrave_base_thread.h" * Base Jaco Arm thread, integrating OpenRAVE @@ -73,6 +75,12 @@ KinovaOpenraveBaseThread::~KinovaOpenraveBaseThread() void KinovaOpenraveBaseThread::init() { + __target_mutex = new Mutex(); + __trajec_mutex = new Mutex(); + + __target_queue = new list< vector<float> >(); + __trajec_queue = new list< vector< vector<float> >* >(); + #ifdef HAVE_OPENRAVE __cfg_OR_use_viewer = config->get_bool("/hardware/jaco/openrave/use_viewer"); __cfg_OR_auto_load_ik = config->get_bool("/hardware/jaco/openrave/auto_load_ik"); @@ -93,6 +101,11 @@ KinovaOpenraveBaseThread::finalize() { unregister_arms(); + delete __target_mutex; + delete __trajec_mutex; + delete __target_queue; + delete __trajec_queue; + #ifdef HAVE_OPENRAVE delete(__OR_robot); __OR_robot = NULL; @@ -103,3 +116,25 @@ KinovaOpenraveBaseThread::finalize() __OR_env = NULL; #endif } + +bool +KinovaOpenraveBaseThread::trajec_ready() +{ + __trajec_mutex->lock(); + bool ready = !__trajec_queue->empty(); + __trajec_mutex->unlock(); + return ready; +} + +vector< vector<float> >* +KinovaOpenraveBaseThread::pop_trajec() +{ + __trajec_mutex->lock(); + vector< vector<float> >* trajec = NULL; + if( !__trajec_queue->empty() ) { + trajec = __trajec_queue->front(); + __trajec_queue->pop_front(); + } + __trajec_mutex->unlock(); + return trajec; +} diff --git a/src/plugins/kinova/openrave_base_thread.h b/src/plugins/kinova/openrave_base_thread.h index 6eff6a0..73a081e 100644 --- a/src/plugins/kinova/openrave_base_thread.h +++ b/src/plugins/kinova/openrave_base_thread.h @@ -32,8 +32,11 @@ #endif #include <string> +#include <list> +#include <vector> namespace fawkes { + class Mutex; typedef struct jaco_arm_struct jaco_arm_t; } @@ -58,7 +61,11 @@ class KinovaOpenraveBaseThread virtual void update_openrave() = 0; - virtual std::vector<float> set_target(float x, float y, float z, float e1, float e2, float e3) = 0; + virtual bool add_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true) = 0; + virtual bool set_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true) = 0; + + virtual bool trajec_ready(); + virtual std::vector< std::vector<float> >* pop_trajec(); protected: /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ @@ -66,6 +73,12 @@ class KinovaOpenraveBaseThread virtual void _init() {} virtual void _load_robot() {} + fawkes::Mutex *__target_mutex; + fawkes::Mutex *__trajec_mutex; + + std::list< std::vector<float> > *__target_queue; + std::list< std::vector< std::vector<float> >* > *__trajec_queue; // list of trajectories. trajectory is a vector of configurations(=vector). + #ifdef HAVE_OPENRAVE fawkes::OpenRaveEnvironment* __OR_env; fawkes::OpenRaveRobot* __OR_robot; diff --git a/src/plugins/kinova/openrave_dual_thread.cpp b/src/plugins/kinova/openrave_dual_thread.cpp index 513867c..c36774e 100644 --- a/src/plugins/kinova/openrave_dual_thread.cpp +++ b/src/plugins/kinova/openrave_dual_thread.cpp @@ -25,6 +25,7 @@ #include "arm.h" #include <interfaces/JacoInterface.h> +#include <core/threading/mutex.h> #include <cmath> #include <stdio.h> @@ -142,13 +143,26 @@ KinovaOpenraveDualThread::unregister_arms() __arms.right = NULL; } -std::vector<float> -KinovaOpenraveDualThread::set_target(float x, float y, float z, float e1, float e2, float e3) +bool +KinovaOpenraveDualThread::add_target(float x, float y, float z, float e1, float e2, float e3, bool plan) { - return std::vector<float>(0); - // no symmetric planning implemented yet + // no IK-solving for coordinated bimanual movement implemented yet + return false; } +bool +KinovaOpenraveDualThread::set_target(float x, float y, float z, float e1, float e2, float e3, bool plan) +{ + __target_mutex->lock(); + __target_queue->clear(); + __target_mutex->unlock(); + __trajec_mutex->lock(); + __trajec_queue->clear(); + __trajec_mutex->unlock(); + return add_target(x, y, z, e1, e2, e3, plan); +} + + void KinovaOpenraveDualThread::update_openrave() { diff --git a/src/plugins/kinova/openrave_dual_thread.h b/src/plugins/kinova/openrave_dual_thread.h index 12b5978..048b6c4 100644 --- a/src/plugins/kinova/openrave_dual_thread.h +++ b/src/plugins/kinova/openrave_dual_thread.h @@ -41,7 +41,8 @@ class KinovaOpenraveDualThread : public KinovaOpenraveBaseThread virtual void update_openrave(); - virtual std::vector<float> set_target(float x, float y, float z, float e1, float e2, float e3); + virtual bool add_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true); + virtual bool set_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true); protected: /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ diff --git a/src/plugins/kinova/openrave_single_thread.cpp b/src/plugins/kinova/openrave_single_thread.cpp index 482b277..edc6af2 100644 --- a/src/plugins/kinova/openrave_single_thread.cpp +++ b/src/plugins/kinova/openrave_single_thread.cpp @@ -24,6 +24,7 @@ #include "types.h" #include <interfaces/JacoInterface.h> +#include <core/threading/mutex.h> #include <cmath> #include <stdio.h> @@ -38,6 +39,7 @@ #endif using namespace fawkes; +using namespace std; /** @class KinovaOpenraveSingleThread "openrave_single_thread.h" * Jaco Arm thread for single-arm setup, integrating OpenRAVE @@ -185,10 +187,10 @@ KinovaOpenraveSingleThread::update_openrave() #endif } -std::vector<float> -KinovaOpenraveSingleThread::set_target(float x, float y, float z, float e1, float e2, float e3) +bool +KinovaOpenraveSingleThread::add_target(float x, float y, float z, float e1, float e2, float e3, bool plan) { - std::vector<float> v; + bool solvable = false; #ifdef HAVE_OPENRAVE try { @@ -200,25 +202,57 @@ KinovaOpenraveSingleThread::set_target(float x, float y, float z, float e1, floa __OR_robot->get_planner_params()->vgoalconfig.resize(__OR_robot->get_robot_ptr()->GetActiveDOF()); // get IK from openrave - bool success = __OR_robot->set_target_euler(EULER_ZXZ, x, y, z, e1, e2, e3); - - if( !success ) { - logger->log_warn(name(), "Initiating goto failed, no IK solution found"); - return v; + solvable = __OR_robot->set_target_euler(EULER_ZXZ, x, y, z, e1, e2, e3); + + if( solvable ) { + logger->log_debug(name(), "IK successful!"); + + // get target IK valoues + vector<float> joints; + __OR_robot->get_target().manip->get_angles(joints); + //need next lines, as "target" only stores a OpenRaveManipulator* , so it stores values in OR only!! + __OR_manip->set_angles(joints); + __OR_manip->get_angles_device(joints); + + if( plan ) { + // add this to the target queue for planning + logger->log_debug(name(), "Adding to target_queue for later planning"); + __target_mutex->lock(); + __target_queue->push_back(joints); + __target_mutex->unlock(); + + } else { + // don't plan, consider this the final configuration, i.e. a trajectory with only 1 point + logger->log_debug(name(), "Skip planning, add this to trajec_queue"); + vector< vector<float> > *trajec = new vector< vector<float> >(); + trajec->push_back(joints); + __trajec_mutex->lock(); + __trajec_queue->push_back(trajec); + __trajec_mutex->unlock(); + } + + } else { + logger->log_warn(name(), "No IK solution found for target."); + return solvable; } - logger->log_debug(name(), "IK successful!"); - // get target IK valoues - std::vector<dReal> joints; - __OR_robot->get_target().manip->get_angles(joints); - //need next lines, as "target" only stores a OpenRaveManipulator* , so it stores values in OR only!! - __OR_manip->set_angles(joints); - __OR_manip->get_angles_device(v); } catch( openrave_exception &e) { throw fawkes::Exception("OpenRAVE Exception:%s", e.what()); } #endif - return v; + return solvable; +} + +bool +KinovaOpenraveSingleThread::set_target(float x, float y, float z, float e1, float e2, float e3, bool plan) +{ + __target_mutex->lock(); + __target_queue->clear(); + __target_mutex->unlock(); + __trajec_mutex->lock(); + __trajec_queue->clear(); + __trajec_mutex->unlock(); + return add_target(x, y, z, e1, e2, e3, plan); } diff --git a/src/plugins/kinova/openrave_single_thread.h b/src/plugins/kinova/openrave_single_thread.h index 741a706..4c2bd74 100644 --- a/src/plugins/kinova/openrave_single_thread.h +++ b/src/plugins/kinova/openrave_single_thread.h @@ -46,7 +46,8 @@ class KinovaOpenraveSingleThread : public KinovaOpenraveBaseThread virtual void update_openrave(); - virtual std::vector<float> set_target(float x, float y, float z, float e1, float e2, float e3); + virtual bool add_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true); + virtual bool set_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true); protected: /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ - *commit* 2cdfa8621b084a6a9ee4fa1c5d4674abcf779dcc - - - - - - - - - - Author: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> Date: Thu Aug 7 00:55:14 2014 +0200 Subject: openrave: can set plannerparams by passing const char* now src/plugins/openrave/robot.cpp | 9 +++++++++ src/plugins/openrave/robot.h | 1 + 2 files changed, 10 insertions(+), 0 deletions(-) _Diff for modified files_: diff --git a/src/plugins/openrave/robot.cpp b/src/plugins/openrave/robot.cpp index ed3c05e..39105fb 100644 --- a/src/plugins/openrave/robot.cpp +++ b/src/plugins/openrave/robot.cpp @@ -468,6 +468,15 @@ OpenRaveRobot::set_target_plannerparams(std::string& params) __target.plannerparams = params; } +/** Set additional planner parameters. + * @param params complete string of additional arguments. + */ +void +OpenRaveRobot::set_target_plannerparams(const char* params) +{ + __target.plannerparams = params; +} + /** Set target angles directly. * @param angles vector with angle values */ diff --git a/src/plugins/openrave/robot.h b/src/plugins/openrave/robot.h index 986d5af..a203c40 100644 --- a/src/plugins/openrave/robot.h +++ b/src/plugins/openrave/robot.h @@ -67,6 +67,7 @@ class OpenRaveRobot virtual bool set_target_euler(euler_rotation_t type, float trans_x, float trans_y, float trans_z, float phi, float theta, float psi, bool no_offset = false); virtual bool set_target_ikparam(OpenRAVE::IkParameterization ik_param); virtual void set_target_plannerparams(std::string& params); + virtual void set_target_plannerparams(const char* params); virtual void set_target_angles( std::vector<float>& angles ); virtual bool set_target_object_position(float trans_x, float trans_y, float trans_z, float rot_x); - *commit* 104f80541e25967434d1b838605dd99325b643cc - - - - - - - - - - Author: Bahram Maleki-Fard <maleki-f...@kbsg.rwth-aachen.de> Date: Fri Aug 8 11:33:46 2014 +0200 Subject: kinova: add planning for single arm src/plugins/kinova/act_thread.cpp | 4 +- src/plugins/kinova/openrave_base_thread.cpp | 17 ++++- src/plugins/kinova/openrave_base_thread.h | 1 + src/plugins/kinova/openrave_single_thread.cpp | 98 ++++++++++++++++++++---- src/plugins/kinova/openrave_single_thread.h | 3 + 5 files changed, 102 insertions(+), 21 deletions(-) _Diff for modified files_: diff --git a/src/plugins/kinova/act_thread.cpp b/src/plugins/kinova/act_thread.cpp index acdb9db..618940e 100644 --- a/src/plugins/kinova/act_thread.cpp +++ b/src/plugins/kinova/act_thread.cpp @@ -432,8 +432,8 @@ KinovaActThread::_process_msgs_arm(jaco_arm_t &arm) msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3()); #ifdef HAVE_OPENRAVE logger->log_debug(name(), "%s: CartesianGotoMessage is being passed to openrave", arm.iface->id()); - // add target to OpenRAVE queue, but skip planning for now (not implemented yet) - bool solvable = arm.openrave_thread->add_target(msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3(), false); + // add target to OpenRAVE queue for planning + bool solvable = arm.openrave_thread->add_target(msg->x(), msg->y(), msg->z(), msg->e1(), msg->e2(), msg->e3()); if( !solvable ) { logger->log_warn(name(), "Failed executing CartesianGotoMessage, arm %s and/or thread %s could not find IK solution", arm.arm->get_name().c_str(), arm.openrave_thread->name()); diff --git a/src/plugins/kinova/openrave_base_thread.cpp b/src/plugins/kinova/openrave_base_thread.cpp index 82b8d7a..80bf59c 100644 --- a/src/plugins/kinova/openrave_base_thread.cpp +++ b/src/plugins/kinova/openrave_base_thread.cpp @@ -75,8 +75,9 @@ KinovaOpenraveBaseThread::~KinovaOpenraveBaseThread() void KinovaOpenraveBaseThread::init() { - __target_mutex = new Mutex(); - __trajec_mutex = new Mutex(); + __planning_mutex = new Mutex(); + __target_mutex = new Mutex(); + __trajec_mutex = new Mutex(); __target_queue = new list< vector<float> >(); __trajec_queue = new list< vector< vector<float> >* >(); @@ -101,10 +102,22 @@ KinovaOpenraveBaseThread::finalize() { unregister_arms(); + delete __planning_mutex; delete __target_mutex; delete __trajec_mutex; + __planning_mutex = NULL; + __target_mutex = NULL; + __trajec_mutex = NULL; + delete __target_queue; + __target_queue = NULL; + + for( list< vector< vector<float> >* >::iterator it=__trajec_queue->begin(); it!=__trajec_queue->end(); ++it) { + delete *it; + *it = NULL; + } delete __trajec_queue; + __trajec_queue = NULL; #ifdef HAVE_OPENRAVE delete(__OR_robot); diff --git a/src/plugins/kinova/openrave_base_thread.h b/src/plugins/kinova/openrave_base_thread.h index 73a081e..d790630 100644 --- a/src/plugins/kinova/openrave_base_thread.h +++ b/src/plugins/kinova/openrave_base_thread.h @@ -73,6 +73,7 @@ class KinovaOpenraveBaseThread virtual void _init() {} virtual void _load_robot() {} + fawkes::Mutex *__planning_mutex; fawkes::Mutex *__target_mutex; fawkes::Mutex *__trajec_mutex; diff --git a/src/plugins/kinova/openrave_single_thread.cpp b/src/plugins/kinova/openrave_single_thread.cpp index edc6af2..c09a2e9 100644 --- a/src/plugins/kinova/openrave_single_thread.cpp +++ b/src/plugins/kinova/openrave_single_thread.cpp @@ -21,6 +21,7 @@ */ #include "openrave_single_thread.h" +#include "arm.h" #include "types.h" #include <interfaces/JacoInterface.h> @@ -149,6 +150,28 @@ KinovaOpenraveSingleThread::finalize() { } void +KinovaOpenraveSingleThread::loop() +{ + __planning_mutex->lock(); + __target_mutex->lock(); + if( !__target_queue->empty() ) { + // get the target + std::vector<float> target = __target_queue->front(); + __target_queue->pop_front(); + __target_mutex->unlock(); + + // run planner + _plan_path(target); + __planning_mutex->unlock(); + + } else { + __target_mutex->unlock(); + __planning_mutex->unlock(); + usleep(30e3); // TODO: make this configurable + } +} + +void KinovaOpenraveSingleThread::register_arm(jaco_arm_t *arm) { __arm = arm; @@ -166,23 +189,27 @@ KinovaOpenraveSingleThread::update_openrave() return; #ifdef HAVE_OPENRAVE - try { - __joints.clear(); - __joints.push_back(__arm->iface->joints(0)); - __joints.push_back(__arm->iface->joints(1)); - __joints.push_back(__arm->iface->joints(2)); - __joints.push_back(__arm->iface->joints(3)); - __joints.push_back(__arm->iface->joints(4)); - __joints.push_back(__arm->iface->joints(5)); - - // get target IK values in openrave format - __OR_manip->set_angles_device(__joints); - __OR_manip->get_angles(__joints); - - __robot->SetDOFValues(__joints, 1, __manip->GetArmIndices()); - - } catch( openrave_exception &e) { - throw fawkes::Exception("OpenRAVE Exception:%s", e.what()); + if( __planning_mutex->try_lock() ) { + try { + __joints.clear(); + __joints.push_back(__arm->iface->joints(0)); + __joints.push_back(__arm->iface->joints(1)); + __joints.push_back(__arm->iface->joints(2)); + __joints.push_back(__arm->iface->joints(3)); + __joints.push_back(__arm->iface->joints(4)); + __joints.push_back(__arm->iface->joints(5)); + + // get target IK values in openrave format + __OR_manip->set_angles_device(__joints); + __OR_manip->get_angles(__joints); + + __robot->SetDOFValues(__joints, 1, __manip->GetArmIndices()); + __planning_mutex->unlock(); + + } catch( openrave_exception &e) { + __planning_mutex->unlock(); + throw fawkes::Exception("OpenRAVE Exception:%s", e.what()); + } } #endif } @@ -256,3 +283,40 @@ KinovaOpenraveSingleThread::set_target(float x, float y, float z, float e1, floa __trajec_mutex->unlock(); return add_target(x, y, z, e1, e2, e3, plan); } + +void +KinovaOpenraveSingleThread::_plan_path(std::vector<float> &target) +{ + // Set active manipulator + __robot->SetActiveManipulator(__manip); + + // Set target point for planner (has already passed IK check previously!) + __OR_manip->set_angles_device(target); + __OR_manip->get_angles(target); + //logger->log_debug(name(), "setting target %f %f %f %f %f %f", + // target.at(0), target.at(1), target.at(2), target.at(3), target.at(4), target.at(5)); + __OR_robot->set_target_angles(target); + + // Set starting point for planner, convert encoder values to angles if necessary + std::vector<float> joints; + __arm->arm->get_joints(joints); + __OR_manip->set_angles_device(joints); + + // Set planning parameters (none yet) + __OR_robot->set_target_plannerparams(""); + + // Run planner + float sampling = 0.01f; //maybe catch from config? or "learning" depending on performance? + try { + openrave->run_planner(__OR_robot, sampling); + } catch (fawkes::Exception &e) { + logger->log_warn(name(), "Planning failed (ignoring): %s", e.what_no_backtrace()); + } + + // add trajectory to queue + //logger->log_debug(name(), "plan successful, adding to queue"); + __trajec_mutex->lock(); + std::vector< std::vector<float> > *trajec = __OR_robot->get_trajectory_device(); + __trajec_queue->push_back(trajec); + __trajec_mutex->unlock(); +} diff --git a/src/plugins/kinova/openrave_single_thread.h b/src/plugins/kinova/openrave_single_thread.h index 4c2bd74..a4053a1 100644 --- a/src/plugins/kinova/openrave_single_thread.h +++ b/src/plugins/kinova/openrave_single_thread.h @@ -31,6 +31,7 @@ #endif #include <string> +#include <vector> class KinovaOpenraveSingleThread : public KinovaOpenraveBaseThread { @@ -39,6 +40,7 @@ class KinovaOpenraveSingleThread : public KinovaOpenraveBaseThread KinovaOpenraveSingleThread(const char *name, const char *manipname, bool load_robot=true); virtual void once(); + virtual void loop(); virtual void finalize(); virtual void register_arm(fawkes::jaco_arm_t *arm); @@ -55,6 +57,7 @@ class KinovaOpenraveSingleThread : public KinovaOpenraveBaseThread private: void _load_robot(); + void _plan_path(std::vector<float> &target); fawkes::jaco_arm_t *__arm; -- Fawkes Robotics Framework http://www.fawkesrobotics.org _______________________________________________ fawkes-commits mailing list fawkes-commits@lists.kbsg.rwth-aachen.de https://lists.kbsg.rwth-aachen.de/listinfo/fawkes-commits
