-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtrajectory_generation.orogen
165 lines (123 loc) · 9.39 KB
/
trajectory_generation.orogen
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
name "trajectory_generation"
using_library "reflexxes"
using_library "joint_control_base"
using_library "base-types"
import_types_from "base"
import_types_from "base/samples/RigidBodyStateSE3.hpp" # Workaround until RigidBodyStateSE3 is in base/orogen/types
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).
# "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 acceleration and maximum jerk (derivative of acceleration, again only Reflexxes TypeIV). Note that RML
# is meant to be used ONLY for reactive motions with quickly changing, but discrete target points. Examples are sensor-based (e.g. Visual Servoing) or
# point-to-point motions. RML is not meant to be used for interpolating full trajectories.
#
# Note: The quality of the trajectory depends on the accuracy of this component's period. Real-time systems may significantly improve performance.
# Also not that the robot should always be able to follow the given trajectory. The current state of the robot will NOT be considered at runtime,
# simply because RML is not meant to be used this way.
#
# Before using this component, please refer to the RML documentation on http://www.reflexxes.ws
#
task_context "RMLTask" do abstract
needs_configuration
runtime_states "FOLLOWING", # A new target has been given and RML is attempting to reach that target. This is indicated by the RML OTG
# (online trajectory generation) algorithm (RML_WORKING). See the rml_result_value port for the current rml result value.
"REACHED", # The given target has been reached. This is indicated by the RML OTG algorithm (RML_FINAL_STATE_REACHED).
# Check the rml_result_value output port for the current rml result value.
"NO_CURRENT_STATE", # Missing current state input (joint_state/cartesian_state). No output command will be generated.
"NO_TARGET" # Missing target input. No output command will be generated.
error_states "RML_ERROR" # RML result is an error state. Check the rml_result_value output port for the current rml
# result value. See ReflexxesAPI.h for possible rml result values
# 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", "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
# - POSITIONAL_LIMITS_ERROR_MSG_ONLY: Component will go into error state if target is out of bounds
# - POSITIONAL_LIMITS_ACTIVELY_PREVENT: Reflexxes will provide a smooth transition at the bounds and avoid exceeding them
# See reflexxes/RMLFlags.h for further details.
property "positional_limits_behavior", "trajectory_generation/PositionalLimitsBehavior", :POSITIONAL_LIMITS_ACTIVELY_PREVENT
# Synchronization behavior between different elements. Can be one of PHASE_SYNCHRONIZATION_IF_POSSIBLE, ONLY_TIME_SYNCHRONIZATION,
# 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"
# Parameters given as input to the current call of the RML OTG Algorithm
output_port "rml_input_parameters", "trajectory_generation/ReflexxesInputParameters"
# Output parameters of the current call of the RML OTG Algorithm.
output_port "rml_output_parameters", "trajectory_generation/ReflexxesOutputParameters"
# Computation time needed for one cycle
output_port "computation_time", "double"
# Difference between two consecutive calls of updateHook(). The value given on this port should match as closely as possible the configured cycle time.
output_port "actual_cycle_time", "double"
# This value has to be the same as the cycle_time property. Don't forget to change the cycle_time when you change the period.
periodic 0.01
end
# Position based implementation in joint space
task_context "RMLPositionTask", subclasses: "RMLTask" do
# Current joint state. Must have valid position entries. Has to contain all joint names configured in the motion_constraints property
input_port "joint_state", "base/samples/Joints"
# Target joint position. Must contain valid position and (optionally) speed entries. The given joint names have to be a subset of the names in the motion_constraints property.
input_port "target", "base/commands/Joints"
# 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", "joint_control_base/ConstrainedJointsCmd"
# Output trajectory. Joint positions, velocities and accelerations
output_port "command", "base/commands/Joints"
# Internal interpolator state (position/speed/acceleration)
output_port "current_sample", "base/samples/Joints"
end
# Velocity based implementation in joint space
task_context "RMLVelocityTask", subclasses: "RMLTask" do
# Velocity reference timeout in seconds: If no new reference arrives for this amount of time, the target velocity will be set to zero.
# Set to .inf to disable timeout
property "no_reference_timeout", "double"
# Convert the output command to a position based trajectory
property "convert_to_position", "bool", false
# Current joint state. Must have valid position entries. Has to contain all joint names configured in the motion_constraints property
input_port "joint_state", "base/samples/Joints"
# Target joint position. Must contain valid speed entries. The given joint names have to be a subset of the names in the motion_constraints property.
input_port "target", "base/commands/Joints"
# 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", "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.
output_port "command", "base/commands/Joints"
# Internal interpolator state (position/speed/acceleration)
output_port "current_sample", "base/samples/Joints"
end
# Position based implementation in Cartesian space
task_context "RMLCartesianPositionTask", subclasses: "RMLTask" do
# Current Cartesian state. Must have valid position/orientation entries!
input_port "cartesian_state", "base/samples/RigidBodyStateSE3"
# Target twist. Must have valid position/orientation and (optionally)velocity/angular_velocity entries!
input_port "target", "base/samples/RigidBodyState"
# Output trajectory (cartesian poses, twists and accelerations)
output_port "command", "base/samples/RigidBodyStateSE3"
# Internal interpolator state (position/speed/acceleration)
output_port "current_sample", "base/samples/RigidBodyStateSE3"
end
# Velocity based implementation in Cartesian space
task_context "RMLCartesianVelocityTask", subclasses: "RMLTask" do
# Velocity reference timeout in seconds: If no new reference arrives for this amount of time, the target velocity will be set to zero.
# Set to .inf to disable timeout
property "no_reference_timeout", "double", 1.0
# Convert the output command to a position based trajectory
property "convert_to_position", "bool", false
# Current Cartesian state. Must have valid position/orientation entries!
input_port "cartesian_state", "base/samples/RigidBodyStateSE3"
# Target twist. Must have valid velocity/angular_velocity entries!
input_port "target", "base/samples/RigidBodyState"
# Output trajectory (cartesian poses, twists and accelerations)
output_port "command", "base/samples/RigidBodyStateSE3"
# Internal interpolator state (position/speed/acceleration)
output_port "current_sample", "base/samples/RigidBodyStateSE3"
end