Re: Motion control stack
Bert Willaert <[email protected]>
| Newsgroups | gmane.science.robotics.orocos.devel |
|---|---|
| Message-ID | <CALDGWE-4V3grqTgz5eiytn8CABtdoxWBo7rb1sif48R0QOSJWA@mail.gmail.com> |
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. Please neglect the patch I send one/two weeks ago. Bert > > On Fri, Dec 14, 2012 at 9:14 AM, Bert Willaert < > [email protected]> wrote: > >> In the meantime, i have some minor extra changes. I ' ll send a new patch >> later today. >> >> bert >> >> >> On Thursday, December 13, 2012, Peter Soetens <[email protected]> >> wrote: >> > On Tue, Nov 27, 2012 at 1:54 PM, Bert Willaert >> > <[email protected]> wrote: >> >> Hi, >> >> >> >> I noticed that the event port from the nAxisGenerator is actually not >> >> implemented. In attachment you can find a patch with an implementation >> for >> >> the port. The events I added are: >> >> >> >> configured_event, moving_event and move_finished_event. >> >> >> >> Especially the last one was what I needed. >> > >> > Thanks for the improvement. Who's picking up this patch and merging it >> in ? >> > >> > Peter >> > >> > > -- 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 KB)
From baed959544b137d5ce5a70faa234e4ce3cd7cef6 Mon Sep 17 00:00:00 2001
From: Bert Willaert <bwillaert@pma-robot-lwr.(none)>
Date: Fri, 14 Dec 2012 15:22:33 +0100
Subject: [PATCH] event_port + namespace + startHook CartImpCtrl
---
.../src/CartesianGeneratorPos.cpp | 13 ++++++++--
.../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 | 25 ++++++++++++-------
naxes_motion_control/src/nAxesGeneratorPos.hpp | 8 ++++--
7 files changed, 44 insertions(+), 23 deletions(-)
diff --git a/cartesian_motion_control/src/CartesianGeneratorPos.cpp b/cartesian_motion_control/src/CartesianGeneratorPos.cpp
index dd78a3b..823d198 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)),
+ moving_event("e_"+name+"_moving"),
+ 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,9 @@ 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);
+
+ // send moving_event (while moving)
+ event_port.write(moving_event);
}
// convert to geometry msgs and send.
diff --git a/cartesian_motion_control/src/CartesianGeneratorPos.hpp b/cartesian_motion_control/src/CartesianGeneratorPos.hpp
index ebb6345..fa5a134 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 moving_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..e7a2886 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)
- {
+ moving_event("e_"+name+"_moving"),
+ 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();
}
@@ -157,6 +162,8 @@ namespace motion_control
p_d.positions[i] = motion_profile[i].Pos( time_passed );
v_d.velocities[i] = motion_profile[i].Vel( time_passed );
}
+ // send moving_event (while moving)
+ event_port.write(moving_event);
}
p_d_port.write( p_d );
v_d_port.write( v_d );
@@ -208,7 +215,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;
@@ -326,4 +333,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..885614c 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 moving_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