Re: Motion control stack
Bert Willaert <[email protected]>
| Newsgroups | gmane.science.robotics.orocos.devel |
|---|---|
| Message-ID | <CALDGWE-W19WC4v1tC-WdesA3e+xsBLT1-FuuoXSyDC0O9c1NGg@mail.gmail.com> |
On Mon, Dec 17, 2012 at 1:37 PM, Herman Bruyninckx < [email protected]> wrote: > On Mon, 17 Dec 2012, Bert Willaert wrote: > > On Sun, Dec 16, 2012 at 6:38 PM, Herman Bruyninckx < >> Herman.Bruyninckx@mech.**kuleuven.be <[email protected]>> >> wrote: >> On Fri, 14 Dec 2012, Bert Willaert wrote: >> >> Hey, >> >> In attachment there is a patch that proposes some changes to >> the >> motion_control stack. The >> changes are: >> >> 1) namespace for all libraries is now MotionControl (for >> naxes it was >> motion_control) >> >> 2) startHook of CartesianImpedanceController: same >> functionality but >> more readable >> >> 3) both nAxes and Cartesian Generators now have an "events" >> port. While >> moving an >> "e_"+name+"_moving" event is sent continuously and when the >> motion is >> finished an >> "e_"+name+"_move_finished" event is sent once. This last >> change has been >> verified only by using >> the moveTo operation, but I assume it only affects that >> operation. >> >> >> I am against the use of an event that is sent out continuously! Events >> should be, well..., _events_, that is things that "happen" once in a >> while. >> > > True. I could change this to an "e_"+name+"_move_started" event that is >> send once. Is that more >> desirable? >> > > It is more "event like" :-) There is a new patch in attachment (that contains all the changes, so again neglect the previous one). The 'e_name_moving' event has been changed to an 'e_name_move_started' event that is send each time the moveTo command is called succesfully. > > > But your _intention_ is very good: while a motion is going in, >> there should >> be a continuous "Quality of Service" data flow going on, which has >> the >> information that you want to put in the "e_"+name+"_moving" event, >> and even >> more. >> >> Having the current implementation in mind, what can this QoS be? >> > > The current implementation is, indeed, very poor in providing QoS data! But > that should be changed, in our more modern "constraint-based" motion > control :-) There, the obvious QoS measures are the ones that indicate how > much each constraint is violated; and a violation that exceeds a specified > threshold then gives rise to an event. > > > The name ""e_"+name+"_move_finished" is also not so good: the event >> should >> indicate the _condition_ that caused the motion to stop, because >> there can >> be many such causes. >> >> Related to the question above: as far as I understand, there is at this >> moment only one >> condition that stops the motion, i.e. when the motion_profile has ended. >> Also, I see there is a pause() command, but there seems to be no way to >> restart the motion >> without doing a reset. Is that correct? >> > > These remarks are most certainly very valid. Again, they have to be solved > by a paradigm shift, towards the above-mentioned "constraint-based" motion > control. In that context, "pause()" can also be a lot more clearly defined. > > > Please neglect the patch I send one/two weeks ago. >> >> Bert >> > > Herman > -- Orocos-Dev mailing list [email protected] http://lists.mech.kuleuven.be/mailman/listinfo/orocos-dev
0001-event_port-namespace-startHook-CartImpCtrl.patch
(application/octet-stream, 10.1 KB)
From a01866ab623230e1cecefc436512d95ff9f73a35 Mon Sep 17 00:00:00 2001 From: Bert Willaert <[email protected]> Date: Fri, 14 Dec 2012 15:22:33 +0100 Subject: [PATCH] event_port + namespace + startHook CartImpCtrl --- .../src/CartesianGeneratorPos.cpp | 14 ++++++++-- .../src/CartesianGeneratorPos.hpp | 4 ++- .../src/CartesianImpedanceController.hpp | 11 +++++--- naxes_motion_control/src/nAxesControllerPos.cpp | 4 +- naxes_motion_control/src/nAxesControllerPos.hpp | 2 +- naxes_motion_control/src/nAxesGeneratorPos.cpp | 26 ++++++++++++------- naxes_motion_control/src/nAxesGeneratorPos.hpp | 8 ++++-- 7 files changed, 45 insertions(+), 24 deletions(-) diff --git a/cartesian_motion_control/src/CartesianGeneratorPos.cpp b/cartesian_motion_control/src/CartesianGeneratorPos.cpp index dd78a3b..1e55e50 100644 --- a/cartesian_motion_control/src/CartesianGeneratorPos.cpp +++ b/cartesian_motion_control/src/CartesianGeneratorPos.cpp @@ -30,7 +30,10 @@ namespace MotionControl CartesianGeneratorPos::CartesianGeneratorPos(string name) : TaskContext(name, PreOperational), m_motion_profile(6, - VelocityProfile_Trap(0, 0)), m_is_moving(false) + VelocityProfile_Trap(0, 0)), + move_started_event("e_"+name+"_move_started"), + move_finished_event("e_"+name+"_move_finished"), + m_is_moving(false) { //Creating TaskContext @@ -38,7 +41,7 @@ CartesianGeneratorPos::CartesianGeneratorPos(string name) : this->addPort("CartesianPoseMsr", m_position_meas_port); this->addPort("CartesianPoseDes", m_position_desi_port); this->addPort("CartesianTwistDes", m_velocity_desi_port); - this->addPort("moveFinished", m_move_finished_port); + this->addPort("events", event_port); //Adding Properties this->addProperty("max_vel", m_gm_maximum_velocity).doc( @@ -107,8 +110,9 @@ void CartesianGeneratorPos::updateHook() // set end position m_position_desi_local = m_traject_end; SetToZero(m_velocity_desi_local); - m_move_finished_port.write(true); m_is_moving = false; + // send move_finished_event (once) + event_port.write(move_finished_event); } else { // position m_velocity_delta = Twist( Vector( m_motion_profile[0].Pos(m_time_passed), @@ -125,6 +129,7 @@ void CartesianGeneratorPos::updateHook() // velocity for (unsigned int i = 0; i < 6; i++) m_velocity_desi_local(i) = m_motion_profile[i].Vel(m_time_passed); + } // convert to geometry msgs and send. @@ -173,6 +178,9 @@ bool CartesianGeneratorPos::moveTo(geometry_msgs::Pose gm_pose, double time) m_time_passed = 0; m_is_moving = true; + // send move_started_event ) + event_port.write(move_started_event); + return true; } diff --git a/cartesian_motion_control/src/CartesianGeneratorPos.hpp b/cartesian_motion_control/src/CartesianGeneratorPos.hpp index ebb6345..018e9fe 100644 --- a/cartesian_motion_control/src/CartesianGeneratorPos.hpp +++ b/cartesian_motion_control/src/CartesianGeneratorPos.hpp @@ -79,6 +79,8 @@ namespace MotionControl double m_max_duration; bool m_is_moving,m_once; + std::string move_started_event; + std::string move_finished_event; protected: /// Dataport containing the current measured end-effector @@ -94,7 +96,7 @@ namespace MotionControl /// MotionControl::CartesianControllerVel RTT::OutputPort< geometry_msgs::Twist > m_velocity_desi_port; - RTT::OutputPort<bool> m_move_finished_port; + RTT::OutputPort<std::string> event_port; }; // class } //namespace diff --git a/cartesian_motion_control/src/CartesianImpedanceController.hpp b/cartesian_motion_control/src/CartesianImpedanceController.hpp index 6529dbc..99cce41 100644 --- a/cartesian_motion_control/src/CartesianImpedanceController.hpp +++ b/cartesian_motion_control/src/CartesianImpedanceController.hpp @@ -76,11 +76,14 @@ namespace MotionControl{ private: bool startHook(){ - if(port_pose_meas.read(m_pose_desi) == RTT::NoData ) + // Starting fails when no data on "CartesianSensorPosition" port + if(port_pose_meas.read(m_pose_meas) == RTT::NoData ) return false; - - port_twist_meas.read(m_twist_desi); - + port_twist_meas.read(m_twist_meas); + // initialize m_pose/twist_desit with 'safe' values + m_pose_desi = m_pose_meas; + m_twist_desi = m_twist_meas; + port_pose_desi.clear(); port_twist_desi.clear(); return true; diff --git a/naxes_motion_control/src/nAxesControllerPos.cpp b/naxes_motion_control/src/nAxesControllerPos.cpp index 1e69f01..1c4a464 100644 --- a/naxes_motion_control/src/nAxesControllerPos.cpp +++ b/naxes_motion_control/src/nAxesControllerPos.cpp @@ -22,7 +22,7 @@ #include "nAxesControllerPos.hpp" #include <rtt/Component.hpp> -namespace motion_control +namespace MotionControl { using namespace RTT; @@ -134,4 +134,4 @@ namespace motion_control }//namespace -ORO_CREATE_COMPONENT( motion_control::nAxesControllerPos ) +ORO_CREATE_COMPONENT( MotionControl::nAxesControllerPos ) diff --git a/naxes_motion_control/src/nAxesControllerPos.hpp b/naxes_motion_control/src/nAxesControllerPos.hpp index ab645b3..4320144 100644 --- a/naxes_motion_control/src/nAxesControllerPos.hpp +++ b/naxes_motion_control/src/nAxesControllerPos.hpp @@ -31,7 +31,7 @@ #include <sensor_msgs/typekit/Types.hpp> #include <motion_control_msgs/typekit/Types.hpp> -namespace motion_control +namespace MotionControl { /** * This component can control the positions of multiple axes. It diff --git a/naxes_motion_control/src/nAxesGeneratorPos.cpp b/naxes_motion_control/src/nAxesGeneratorPos.cpp index 81f66dd..2c69b88 100644 --- a/naxes_motion_control/src/nAxesGeneratorPos.cpp +++ b/naxes_motion_control/src/nAxesGeneratorPos.cpp @@ -22,7 +22,7 @@ #include <rtt/Component.hpp> #include <rtt/os/MutexLock.hpp> -namespace motion_control +namespace MotionControl { using namespace RTT; using namespace KDL; @@ -31,8 +31,12 @@ namespace motion_control nAxesGeneratorPos::nAxesGeneratorPos(const string& name) : TaskContext(name,PreOperational), - finished_event(name+"move_finished"), traj_finished_event(name+"traj_finished"), is_moving(false), isTrajMoving(false), trajIndex(0) - { + move_started_event("e_"+name+"_move_started"), + move_finished_event("e_"+name+"_move_finished"), traj_finished_event("e_"+name+"_traj_finished"), + + is_moving(false), isTrajMoving(false), trajIndex(0) + +{ //Creating TaskContext //Adding properties @@ -45,7 +49,7 @@ namespace motion_control this->addPort("nAxesSensorPosition" , p_m_port ); this->addPort("nAxesDesiredPosition" , p_d_port ); this->addPort("nAxesDesiredVelocity" , v_d_port ); - this->addPort("moveFinished", move_finished_port); + this->addPort("events", event_port); this->addEventPort("nAxesJointPosition" , joint_endpose_port, boost::bind(&nAxesGeneratorPos::moveToOnPort, this)); this->addEventPort("nAxesJointPositionDelayed" , joint_endpose_delayed_port, boost::bind(&nAxesGeneratorPos::moveToDelayedOnPort, this)); @@ -106,7 +110,7 @@ namespace motion_control p_d_port.setDataSample( p_d ); v_d.velocities.assign(num_axes,0); v_d_port.setDataSample( v_d ); - move_finished_port.setDataSample(finished_event); + event_port.setDataSample(move_finished_event); return true; } @@ -146,9 +150,10 @@ namespace motion_control for (unsigned int i=0; i<num_axes; i++){ p_d.positions[i] = motion_profile[i].Pos( max_duration ); v_d.velocities[i] = motion_profile[i].Vel( max_duration ); - is_moving = false; - move_finished_port.write(finished_event); } + is_moving = false; + // send move_finished_event (once) + event_port.write(move_finished_event); if (isTrajMoving == true) { this->moveTraject(); } @@ -208,7 +213,7 @@ namespace motion_control this->moveTo(traject.points[trajIndex].positions, 0.1); } else { - move_finished_port.write(traj_finished_event); + event_port.write(traj_finished_event); isTrajMoving = false; } return true; @@ -249,7 +254,8 @@ namespace motion_control time_passed = 0; is_moving = true; - + // send move_started_event + event_port.write(move_started_event); return true; } @@ -326,4 +332,4 @@ namespace motion_control } }//namespace -ORO_CREATE_COMPONENT( motion_control::nAxesGeneratorPos ) +ORO_CREATE_COMPONENT( MotionControl::nAxesGeneratorPos ) diff --git a/naxes_motion_control/src/nAxesGeneratorPos.hpp b/naxes_motion_control/src/nAxesGeneratorPos.hpp index 585c2d3..ab3fd58 100644 --- a/naxes_motion_control/src/nAxesGeneratorPos.hpp +++ b/naxes_motion_control/src/nAxesGeneratorPos.hpp @@ -34,7 +34,7 @@ #include <trajectory_msgs/typekit/Types.hpp> -namespace motion_control +namespace MotionControl { /** * This component generates paths between the current positions @@ -163,7 +163,7 @@ namespace motion_control /// DataPort containing the current desired velocity. RTT::OutputPort< motion_control_msgs::JointVelocities > v_d_port; /// DataPort that will be written to when the motion is finished - RTT::OutputPort <std::string > move_finished_port; + RTT::OutputPort <std::string > event_port; private: @@ -173,7 +173,9 @@ namespace motion_control double max_duration; bool is_moving; - std::string finished_event; + std::string move_started_event; + std::string move_finished_event; + protected: /// Vector with the maximum velocity of each axis std::vector<double> v_max_prop; -- 1.7.4.1