Merge branch 'windup'

scripts/config/trajectory_generation::RMLVelocityTask.yml

@@ -3,8 +3,8 @@
 # maximum maximum speed, maximum acceleration and maximum jerk (derivative of acceleration).
 motion_constraints:
   names: ["Joint1", "Joint2"]
-  elements: [{max: {position: 1.0, speed: 0.3, acceleration: 0.5}, min: {position: -1.0}, max_jerk: 1.0},
-             {max: {position: 1.5, speed: 0.6, acceleration: 1.0}, min: {position: -1.0}, max_jerk: 1.0}]
+  elements: [{max: {position: 10.0, speed: 0.3, acceleration: 50}, min: {position: -10.0}, max_jerk: 200.0},
+             {max: {position: 10.5, speed: 0.6, acceleration: 50}, min: {position: -10.0}, max_jerk: 200.0}]
 
 # Behaviour on the position limits (only reflexxes TypeIV!!!). Can be one of POSITIONAL_LIMITS_IGNORE, POSITIONAL_LIMITS_ERROR_MSG_ONLY
 # and POSITIONAL_LIMITS_ACTIVELY_PREVENT. See reflexxes/RMLFlags.h for details.
@@ -20,3 +20,10 @@ no_reference_timeout: .inf
 
 # Converts the output to position commands
 convert_to_position: true
+
+--- name:windup
+# Max. integrator windup. Size has to be same as number of joints or empty, in which case no windup is used.
+# Only used if convert_to_position is set ot true. Output velocity will be set to zero if
+# the difference between actual position and interpolator position is bigger than the given windup.
+max_pos_diff:
+    data: [0.1, 0.1]

scripts/test_interpolator_windup.rb

@@ -0,0 +1,70 @@
+require 'orocos'
+require 'readline'
+require 'csv'
+
+Orocos.initialize
+Orocos.conf.load_dir('config')
+
+Orocos.run "trajectory_generation::RMLVelocityTask" => "interpolator" do
+
+    interpolator = Orocos::TaskContext.get "interpolator"
+    Orocos.conf.apply(interpolator, ["default", "windup"], true)
+    interpolator.configure
+    interpolator.start
+
+    joint_state = Types::Base::Samples::Joints.new
+    joint_state.names = interpolator.motion_constraints.names
+    joint_state.names.each do
+        state = Types::Base::JointState.new
+        state.position = 0.0
+        joint_state.elements << state
+    end
+    joint_state.time = Types::Base::Time.now
+
+    Readline.readline("Press Enter to start")
+
+    joint_state_writer = interpolator.joint_state.writer
+    joint_state_writer.write(joint_state)
+
+    Readline.readline("Press Enter to send target")
+
+    target = Types::Base::Samples::Joints.new
+    target.names = interpolator.motion_constraints.names
+    cmd = Types::Base::JointState.new
+    cmd.speed = 0.3
+    target.elements << cmd
+    cmd = Types::Base::JointState.new
+    cmd.speed = 0.5
+    target.elements << cmd
+
+    target_writer = interpolator.target.writer
+    target_writer.write(target)
+
+    command_reader = interpolator.command.reader
+    start = Types.base.Time.now
+    CSV.open("test.csv", "w", {:col_sep => "\t"}) do |csv|
+        while true
+            command = command_reader.read
+            if command
+                #print "Target velocity: "
+                arr = [(Types.base.Time.now - start).to_s + " "]
+                target.elements.each { |e|  arr << e.speed.to_s + " "}
+                #print "\nCommanded velocity: "
+                command.elements.each { |e| arr << e.speed.to_s + " "}
+                #print "\nCommanded position: "
+                command.elements.each { |e| arr << e.position.to_s + " "}
+                #arr << "\n"
+                csv << arr
+                #puts "\n---------------------------------------------------"
+                joint_state = command
+            end
+            sleep 0.01
+            if (Types.base.Time.now - start) > 5
+                 joint_state_writer.write(joint_state)
+            end
+            if (Types.base.Time.now - start) > 10
+                break
+           end
+        end
+    end
+end

tasks/RMLVelocityTask.cpp

@@ -15,9 +15,17 @@ bool RMLVelocityTask::configureHook(){
     if(base::isNaN(no_reference_timeout))
         no_reference_timeout = base::infinity<double>();
     convert_to_position = _convert_to_position.get();
+    max_pos_diff = _max_pos_diff.get();
 
     if (! RMLVelocityTaskBase::configureHook())
         return false;
+
+    if(max_pos_diff.size() > 0 && max_pos_diff.size() != rml_input_parameters->NumberOfDOFs){
+        LOG_ERROR("%s: Max pos. diff has %i entries but configured number of DOF is %i",
+                  this->getName().c_str(), max_pos_diff.size(), rml_input_parameters->NumberOfDOFs);
+        return false;
+    }
+
     return true;
 }
 
@@ -72,8 +80,24 @@ bool RMLVelocityTask::updateTarget(RMLInputParameters* new_input_parameters){
     return has_target;
 }
 
+void RMLVelocityTask::correctInterpolatorState(RMLInputParameters *in,
+                                               RMLOutputParameters *out,
+                                               const base::samples::Joints &act,
+                                               const base::VectorXd& max_diff){
+
+    target2RmlTypes(target, motion_constraints, *(RMLVelocityInputParameters*)in);
+    for(uint i = 0; i < in->NumberOfDOFs; i++){
+        const base::JointState& js = act.getElementByName(motion_constraints.names[i]);
+        if(fabs(out->NewPositionVector->VecData[i] - js.position) > max_diff(i))
+            in->TargetVelocityVector->VecData[i]     = 0.0;
+    }
+}
+
 ReflexxesResultValue RMLVelocityTask::performOTG(RMLInputParameters* new_input_parameters, RMLOutputParameters* new_output_parameters, RMLFlags *rml_flags){
 
+    if(convert_to_position && max_pos_diff.size() > 0)
+        correctInterpolatorState(new_input_parameters, new_output_parameters, joint_state, max_pos_diff);
+
     int result = rml_api->RMLVelocity(*(RMLVelocityInputParameters*)new_input_parameters,
                                        (RMLVelocityOutputParameters*)new_output_parameters,
                                       *(RMLVelocityFlags*)rml_flags );

tasks/RMLVelocityTask.hpp

@@ -19,6 +19,7 @@ class RMLVelocityTask : public RMLVelocityTaskBase
     double no_reference_timeout;
     base::Time time_of_last_reference;
     bool convert_to_position;
+    base::VectorXd max_pos_diff;
 
 protected:
     /** Update the motion constraints of a particular element*/
@@ -49,6 +50,11 @@ class RMLVelocityTask : public RMLVelocityTaskBase
     /** Convert from RMLOutputParameters to orogen type*/
     virtual const ReflexxesOutputParameters& convertRMLOutputParams(const RMLOutputParameters &in, ReflexxesOutputParameters& out);
 
+    /** Correct the given RMLOutputParameters if the difference between actual joint position and interpolator position is bigger than max_diff*/
+    void correctInterpolatorState(RMLInputParameters *in,
+                                  RMLOutputParameters *out,
+                                  const base::samples::Joints &act,
+                                  const base::VectorXd& max_diff);
 public:
     RMLVelocityTask(std::string const& name = "trajectory_generation::RMLVelocityTask") : RMLVelocityTaskBase(name){}
     RMLVelocityTask(std::string const& name, RTT::ExecutionEngine* engine) : RMLVelocityTaskBase(name, engine){}

trajectory_generation.orogen

@@ -52,6 +52,12 @@ task_context "RMLTask" do abstract
     # ONLY_PHASE_SYNCHRONIZATION and NO_SYNCHRONIZATION. See reflexxes/RMLFlags.h for details.
     property "synchronization_behavior", "RMLFlags/SyncBehaviorEnum", :PHASE_SYNCHRONIZATION_IF_POSSIBLE
 
+
+    # Max. integrator windup. Size has to be same as number of joints or empty, in which case no windup is used.
+    # Only used if convert_to_position is set ot true. Output velocity will be set to zero if
+    # the difference between actual position and interpolator position is bigger than the given windup.
+    property "max_pos_diff", "base/VectorXd"
+
     # Result value of the current call of the RML OTG Algorithm. See ReflexxesAPI.h for possible rml result values
     output_port "rml_result_value", "trajectory_generation/ReflexxesResultValue"