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.*/