diff --git a/manifest.xml b/manifest.xml
index 28721f6..52d8fbf 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -21,4 +21,5 @@
+
diff --git a/tasks/Conversions.cpp b/tasks/Conversions.cpp
index b644bca..ee4a67f 100644
--- a/tasks/Conversions.cpp
+++ b/tasks/Conversions.cpp
@@ -1,6 +1,8 @@
#include "Conversions.hpp"
#include
+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
diff --git a/tasks/Conversions.hpp b/tasks/Conversions.hpp
index d2eff00..46eef44 100644
--- a/tasks/Conversions.hpp
+++ b/tasks/Conversions.hpp
@@ -3,6 +3,8 @@
#include "trajectory_generationTypes.hpp"
#include
+#include
+#include
#include
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);
diff --git a/tasks/RMLTask.hpp b/tasks/RMLTask.hpp
index 657613f..d3d53c5 100644
--- a/tasks/RMLTask.hpp
+++ b/tasks/RMLTask.hpp
@@ -4,7 +4,9 @@
#define TRAJECTORY_GENERATION_RMLTASK_TASK_HPP
#include "trajectory_generation/RMLTaskBase.hpp"
-#include "trajectory_generationTypes.hpp"
+#include
+#include
+#include
#include
/* 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
diff --git a/trajectory_generation.orogen b/trajectory_generation.orogen
index edf1f42..ba0aae6 100644
--- a/trajectory_generation.orogen
+++ b/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.
diff --git a/trajectory_generationTypes.hpp b/trajectory_generationTypes.hpp
index 0e4523a..8a57cc8 100644
--- a/trajectory_generationTypes.hpp
+++ b/trajectory_generationTypes.hpp
@@ -1,7 +1,8 @@
#ifndef trajectory_generation_TYPES_HPP
#define trajectory_generation_TYPES_HPP
-#include
+#include
+#include
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();
- min.position = base::unset();
- max.speed = base::unset();
- max.acceleration = base::unset();
- max_jerk = base::unset();
- }
-
- 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{
-};
-
-/** 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 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.*/