diff --git a/scripts/config/trajectory_generation::RMLVelocityTask.yml b/scripts/config/trajectory_generation::RMLVelocityTask.yml index b40b66a..b1951f5 100644 --- a/scripts/config/trajectory_generation::RMLVelocityTask.yml +++ b/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] diff --git a/scripts/test_interpolator_windup.rb b/scripts/test_interpolator_windup.rb new file mode 100644 index 0000000..898d509 --- /dev/null +++ b/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 diff --git a/tasks/RMLVelocityTask.cpp b/tasks/RMLVelocityTask.cpp index 44f28f5..7cb181d 100644 --- a/tasks/RMLVelocityTask.cpp +++ b/tasks/RMLVelocityTask.cpp @@ -15,9 +15,17 @@ bool RMLVelocityTask::configureHook(){ if(base::isNaN(no_reference_timeout)) no_reference_timeout = base::infinity(); 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 ); diff --git a/tasks/RMLVelocityTask.hpp b/tasks/RMLVelocityTask.hpp index 4ef1456..6e97d79 100644 --- a/tasks/RMLVelocityTask.hpp +++ b/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){} diff --git a/trajectory_generation.orogen b/trajectory_generation.orogen index 0040fcf..bf6122d 100644 --- a/trajectory_generation.orogen +++ b/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"