Move types like MotionConstraint to another library

manifest.xml

@@ -21,4 +21,5 @@
 
   <depend package="base/types" />
   <depend package="control/reflexxes" />
+  <depend package="control/joint_control_base" />
 </package>

tasks/Conversions.cpp

@@ -1,6 +1,8 @@
 #include "Conversions.hpp"
 #include <base-logging/Logging.hpp>
 
+using namespace joint_control_base;
+
 namespace trajectory_generation{
 
 base::Vector3d quaternion2Euler(const base::Orientation& orientation){
@@ -136,8 +138,12 @@ void rmlTypes2CartesianState(const RMLInputParameters& params, base::samples::Ri
 }
 
 void motionConstraint2RmlTypes(const MotionConstraint& constraint, const uint idx, RMLInputParameters& params){
-    constraint.validate(); // Check if constraints are ok, e.g. max.speed > 0 etc
+    // Check if constraints are ok, e.g. speed > 0 etc.
+    constraint.validateVelocityLimit();
+    constraint.validateAccelerationLimit();
+    constraint.validateJerkLimit();
 #ifdef USING_REFLEXXES_TYPE_IV
+    constraint.validatePositionLimits();
     params.MaxPositionVector->VecData[idx] = constraint.max.position;
     params.MinPositionVector->VecData[idx] = constraint.min.position;
 #endif

tasks/Conversions.hpp

@@ -3,6 +3,8 @@
 
 #include "trajectory_generationTypes.hpp"
 #include <base/samples/RigidBodyState.hpp>
+#include <joint_control_base/MotionConstraint.hpp>
+#include <joint_control_base/ConstrainedJointsCmd.hpp>
 #include <ReflexxesAPI.h>
 
 namespace trajectory_generation{
@@ -23,17 +25,17 @@ void rmlTypes2JointState(const RMLInputParameters& params, base::samples::Joints
 void cartesianState2RmlTypes(const base::samples::RigidBodyState& cartesian_state, RMLInputParameters& params);
 void rmlTypes2CartesianState(const RMLInputParameters& params, base::samples::RigidBodyState& cartesian_state);
 
-void motionConstraint2RmlTypes(const MotionConstraint& constraint, const uint idx, RMLInputParameters& params);
-void motionConstraint2RmlTypes(const MotionConstraint& constraint, const uint idx, RMLPositionInputParameters& params);
-void motionConstraint2RmlTypes(const MotionConstraint& constraint, const uint idx, RMLVelocityInputParameters& params);
+void motionConstraint2RmlTypes(const joint_control_base::MotionConstraint& constraint, const uint idx, RMLInputParameters& params);
+void motionConstraint2RmlTypes(const joint_control_base::MotionConstraint& constraint, const uint idx, RMLPositionInputParameters& params);
+void motionConstraint2RmlTypes(const joint_control_base::MotionConstraint& constraint, const uint idx, RMLVelocityInputParameters& params);
 
 void rmlTypes2Command(const RMLPositionOutputParameters& params, base::commands::Joints& command);
 void rmlTypes2Command(const RMLPositionOutputParameters& params, base::samples::RigidBodyState& command);
 void rmlTypes2Command(const RMLVelocityOutputParameters& params, base::commands::Joints& command);
 void rmlTypes2Command(const RMLVelocityOutputParameters& params, base::samples::RigidBodyState& command);
 
-void target2RmlTypes(const ConstrainedJointsCmd& target, const MotionConstraints& default_constraints, RMLPositionInputParameters& params);
-void target2RmlTypes(const ConstrainedJointsCmd& target, const MotionConstraints& default_constraints, RMLVelocityInputParameters& params);
+void target2RmlTypes(const joint_control_base::ConstrainedJointsCmd& target, const joint_control_base::MotionConstraints& default_constraints, RMLPositionInputParameters& params);
+void target2RmlTypes(const joint_control_base::ConstrainedJointsCmd& target, const joint_control_base::MotionConstraints& default_constraints, RMLVelocityInputParameters& params);
 void target2RmlTypes(const base::samples::RigidBodyState& target, RMLPositionInputParameters& params);
 void target2RmlTypes(const base::samples::RigidBodyState& target, RMLVelocityInputParameters& params);
 void target2RmlTypes(const double target_pos, const double target_vel, const uint idx, RMLPositionInputParameters& params);

tasks/RMLTask.hpp

@@ -4,7 +4,9 @@
 #define TRAJECTORY_GENERATION_RMLTASK_TASK_HPP
 
 #include "trajectory_generation/RMLTaskBase.hpp"
-#include "trajectory_generationTypes.hpp"
+#include <joint_control_base/MotionConstraint.hpp>
+#include <joint_control_base/ConstrainedJointsCmd.hpp>
+#include <base/Time.hpp>
 #include <ReflexxesAPI.h>
 
 /* TODOs (D.M, 2016/06/28):
@@ -20,6 +22,10 @@
 
 namespace trajectory_generation{
 
+typedef joint_control_base::MotionConstraint MotionConstraint;
+typedef joint_control_base::MotionConstraints MotionConstraints;
+typedef joint_control_base::ConstrainedJointsCmd ConstrainedJointsCmd;
+
 /** This task generates a feasible, time-stamped trajectory to a given a target (position/velocity, depending on the subclass used).
  *  "Feasible" means here that the output trajectory (command port) will respect the motion constraints defined by the
  *  motion_constraints-property, that is maximum/minimum position (only Reflexxes TypeIV), maximum speed, maximum

trajectory_generation.orogen

@@ -1,9 +1,12 @@
 name "trajectory_generation"
 
 using_library "reflexxes"
+using_library "joint_control_base"
 
 import_types_from "base"
 import_types_from "RMLFlags.h"
+import_types_from "joint_control_base/MotionConstraint.hpp"
+import_types_from "joint_control_base/ConstrainedJointsCmd.hpp"
 import_types_from "trajectory_generationTypes.hpp"
 
 # This task generates a feasible, time-stamped trajectory to a given a target (joint position/velocity or Cartesian position/velocity, depending on the subclass used).
@@ -34,7 +37,7 @@ task_context "RMLTask" do abstract
     # Motion constraints that define the properties of the output trajectory that is sent on the command-port.
     # These include the maximum/minimum position (only Reflexxes TypeIV), maximum maximum speed, maximum acceleration and maximum jerk
     # (derivative of acceleration, only Reflexxes TypeIV).
-    property "motion_constraints", "trajectory_generation/MotionConstraints"
+    property "motion_constraints", "joint_control_base/MotionConstraints"
 
     # Behaviour at the position limits (only reflexxes TypeIV!!!). Can be one of the following:
     #   - POSITIONAL_LIMITS_IGNORE: Positional limits are completely ignored
@@ -77,7 +80,7 @@ task_context "RMLPositionTask", subclasses: "RMLTask" do
 
     # Target joint position/speed + new motion constraints.  If one of the new constraint values (e.g. max.position) is NaN, the default motion
     # constraints given by the motion_constraints property will be applied
-    input_port "constrained_target", "trajectory_generation/ConstrainedJointsCmd"
+    input_port "constrained_target", "joint_control_base/ConstrainedJointsCmd"
 
     # Output trajectory. Joint positions, velocities and accelerations
     output_port "command", "base/commands/Joints"
@@ -104,7 +107,7 @@ task_context "RMLVelocityTask", subclasses: "RMLTask" do
 
     # Target joint speed + new motion constraints.  If one of the new constraint values (e.g. max.position) is NaN, the default motion constraints
     # given by the motion_constraints property will be applied
-    input_port "constrained_target", "trajectory_generation/ConstrainedJointsCmd"
+    input_port "constrained_target", "joint_control_base/ConstrainedJointsCmd"
 
     # Output trajectory. If convert_to_position is set to true, this will contain joint velocities and accelerations. Otherwise
     # only joint velocities and accelerations.

trajectory_generationTypes.hpp

@@ -1,7 +1,8 @@
 #ifndef trajectory_generation_TYPES_HPP
 #define trajectory_generation_TYPES_HPP
 
-#include <base/commands/Joints.hpp>
+#include <vector>
+#include <base/Float.hpp>
 
 namespace trajectory_generation {
 
@@ -12,92 +13,6 @@ enum PositionalLimitsBehavior{
     POSITIONAL_LIMITS_ACTIVELY_PREVENT /** Reflexxes will make a smooth transition at the bounds and prevent exceededing them*/
 };
 
-/** Motion constraints to define the dynamic behavior of the trajectories generated by this component*/
-struct MotionConstraint{
-
-    struct upperLimit{
-        double position;     /** Maximum joint or Cartesian position. In rad or m. (only reflexxes TypeIV, has to be > min. position!) */
-        double speed;        /** Maximum joint or Cartesian velocity. In rad/s or m/s. (has to be > 0) */
-        double acceleration; /** Maximum joint or Cartesian acceleration. In rad/ss or m/ss. (has to be > 0) */
-    };
-    struct lowerLimit{
-        double position;     /** Minimum joint or Cartesian position. In rad or m. (only reflexxes TypeIV, has to be < max. position!) */
-    };
-    double max_jerk;         /** Maximum joint or Cartesian jerk (derivative of acceleration). In rad/sss or m/sss. (has to be > 0) */
-    upperLimit max;
-    lowerLimit min;
-
-    MotionConstraint(){
-        max.position      = base::unset<float>();
-        min.position      = base::unset<float>();
-        max.speed         = base::unset<float>();
-        max.acceleration  = base::unset<float>();
-        max_jerk          = base::unset<float>();
-    }
-
-    bool hasMaxPosition() const {return !base::isUnset(max.position);}
-    bool hasMinPosition() const {return !base::isUnset(min.position);}
-    bool hasMaxVelocity() const {return !base::isUnset(max.speed);}
-    bool hasMaxAcceleration() const {return !base::isUnset(max.acceleration);}
-    bool hasMaxJerk() const {return !base::isUnset(max_jerk);}
-
-    void validate() const{
-#ifdef USING_REFLEXXES_TYPE_IV
-        if(!hasMaxPosition())
-            throw std::invalid_argument("Motion Constraints: Maximum position is invalid");
-        if(!hasMinPosition())
-            throw std::invalid_argument("Motion Constraints: Minimum position is invalid");
-        if(max.position <= min.position)
-            throw std::invalid_argument("Motion Constraints: Max. position has to be > min. position");
-#endif
-        if(!hasMaxVelocity())
-            throw std::invalid_argument("Motion Constraints: Maximum speed is invalid");
-        if(!hasMaxAcceleration())
-            throw std::invalid_argument("Motion Constraints: Maximum acceleration is invalid");
-        if(!hasMaxJerk())
-            throw std::invalid_argument("Motion Constraints: Maximum jerk is invalid");
-        if(max.speed <= 0)
-            throw std::invalid_argument("Motion Constraints: Max. speed has to be > 0");
-        if(max.acceleration <= 0)
-            throw std::invalid_argument("Motion Constraints: Max. acceleration has to be > 0");
-        if(max_jerk <= 0)
-            throw std::invalid_argument("Motion Constraints: Max. jerk has to be > 0");
-    }
-    void applyDefaultIfUnset(const MotionConstraint& default_constraints){
-        if(!hasMaxPosition())
-            max.position = default_constraints.max.position;
-        if(!hasMinPosition())
-            min.position = default_constraints.min.position;
-        if(!hasMaxVelocity())
-            max.speed = default_constraints.max.speed;
-        if(!hasMaxAcceleration())
-            max.acceleration = default_constraints.max.acceleration;
-        if(!hasMaxJerk())
-            max_jerk = default_constraints.max_jerk;
-    }
-};
-
-/** For backward compatibility */
-typedef MotionConstraint JointMotionConstraints;
-
-/** Named vector of MotionConstraints, i.e. motion constraints for all the joints of a robot*/
-struct MotionConstraints : base::NamedVector<MotionConstraint>{
-};
-
-/** For backward compatibility */
-typedef MotionConstraints JointsMotionConstraints;
-
-/** Named vector of Joints command with motion constraints, i.e. constrained commands for all the joints of a robot*/
-struct ConstrainedJointsCmd : public base::commands::Joints{
-    std::vector<MotionConstraint> motion_constraints;
-    void validate() const{
-        if(names.size() != size())
-            throw std::invalid_argument("Invalid target: Size of name vector should be same as element vector");
-        if(!motion_constraints.empty() && motion_constraints.size() != size())
-            throw std::invalid_argument("Invalid target: If not empty, motion constraints must have same size as target element vector");
-    }
-};
-
 /** Result values of the Online Trajectory Generation algorithm. See reflexxes/ReflexxesAPI.h for further details*/
 enum ReflexxesResultValue{
     RML_WORKING	                            =  0,   /** The Online Trajectory Generation algorithm is working; the final state of motion has not been reached yet.*/