forked from rock-control/control-orogen-cart_ctrl_wdls
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathcart_ctrl_wdls.orogen
179 lines (136 loc) · 7.48 KB
/
cart_ctrl_wdls.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
166
167
168
169
170
171
172
173
174
175
176
177
178
name "cart_ctrl_wdls"
import_types_from "kdl"
import_types_from "base"
using_library "base-logging"
using_library "kdl_parser"
using_library "urdfdom"
using_library "sdformat"
using_library "kdl_conversions"
import_types_from 'kdl_parser/RobotModelFormat.hpp' # for the robot model format
#
# Weighted Damped Least Squares Solver from KDL
#
task_context "WDLSSolver" do
needs_configuration
cycle_time = 0.01
####### Required properties:
# DEPRECATED use robot_model and robot_model_format instead
property("urdf_file", "/std/string")
# Filename or text representation of the robot model
#
# By default, it is interpreted as a path to a URDF file. If it starts with
# an XML tag, it is interpreted instead as a XML text.
#
# Whether it is a URDF or SDF model is controlled by the robot_model_format
# property
property("robot_model", "/std/string")
# In which format is robot_model
#
# Set to ROBOT_MODEL_URDF for URDF and ROBOT_MODEL_SDF for SDF.
property("robot_model_format", "/kdl_parser/ROBOT_MODEL_FORMAT", :ROBOT_MODEL_URDF)
property("root", "/std/string").
doc("Root frame of the kinematic chain. Has to be a link in the given urdf model.")
property("tip", "/std/string").
doc("Tip frame of the kinematic chain. Has to be a link in the given urdf model.")
property("lambda", "double").
doc("Damping term. Reduces the output velocity in singular configurations. This value, and epsilon, should be chosen with extreme care. Depends on kinematic structure of the robot. Reasonable value for aila's right arm was 0.1")
####### Optional properties:
property("input_in_tip_coordinates", "bool", false).
doc("If true, the input twist is assumed to be in tip coordinates, otherwise in root coordinates. Note that the center of rotation will always assumed to be equal to the chosen reference frame. E.g. if the tip is chosen as reference frame, not only the rotational and translational velocity will be expressed in tip coordinates, but also the the center of rotation will be equal to the center of tip frame")
property("epsilon", "double", 1e-5).
doc("If a singular value is smaller than this, the damping term will be activated. If you want damping in wider range of configurations, increase this value.")
property("weights_js", "/base/VectorXd").
doc("Weights in task space in the range 0 ... 1. Size has to be same as number of joints in kinematic chain. Zero weight means that the corresponding joint will not be used at all. If not set, all weights are set by default to 1.")
property("weights_ts", "/base/VectorXd").
doc("Weights in task space in the range 0 ... 1. Size has to be 6. Zero weight means that the corresponding task space direction will not be used at all. If not set, all values are set by default to 1.")
######## Ports:
input_port("desired_twist", "/base/samples/RigidBodyState").
doc("Input twist (Translational and rotational Cartesian velocity) for the solver.")
input_port("joint_status", "/base/samples/Joints").
doc("Current joint state. Joint names will be mapped to the joints of the kinematic chain, so the input may actually contain more joint that the kinematic chain.")
output_port("solver_output", "/base/commands/Joints").
doc("Output velocity in joint space (IK solution).")
output_port("cartesian_status", "/base/samples/RigidBodyState").
doc("Current Pose and twist of the tip of the kinematic chain withn respect root frame.")
periodic cycle_time
end
#
# Alternative implementation of the WDLSController. Computes damping online according to current manipulability:
# lambda = lambda_0 * (1 - m / m_0)^2 where m_0 is the maximum manipulability is the workspace of the robot. This
# should lead to a smoother and more secure motion, since the transition from damped to undamped behavior is omitted
# and the damping is not constant for all configurations.
# Lambda_0 will be set equal to the lambda property value (see WDLSSolver). Epsilon should be set to a very high value,
# so that the damping term is always active.
#
task_context "AdaptiveWDLSSolver" do
subclasses "cart_ctrl_wdls::WDLSSolver"
needs_configuration
####### Optional properties:
property("max_manipulability", "double").
doc("Maximum Manipulability value in the workspace of the robot. Used as normalization term when computing the damping factor. If unknown, leave empty, so that the value will be computed online, which may lead to slow velocities at startup.")
####### Ports:
output_port("manipulability", "double").
doc("Current manipulability of the kinematic chain.")
output_port("cur_lambda", "double").
doc("Current damping term.")
end
#
# Cartesian Pose Controller
#
task_context "CartCtrl" do
needs_configuration
cycle_time = 0.01
runtime_states 'FOLLOWING'
####### Optional properties:
property("cycle_time", "double", cycle_time).
doc("Control cycle time in seconds.")
property("p_gain", "/std/vector<double>").
doc("Proportional Gain for the controller. Size has to be 6. If not set, all values default to 1.")
property("max_ctrl_out", "/base/samples/RigidBodyState").
doc("Max controller output twist. If one value exceeds this maximum, all other values will be scaled accordingly. If not set, arbitrarily high controller output is allowed.")
####### Ports:
input_port("command", "/base/samples/RigidBodyState").
doc("Desired pose (controller reference)")
input_port("cartesian_status", "/base/samples/RigidBodyState").
doc("Current pose")
output_port("ctrl_error", "/base/samples/RigidBodyState").
doc("Debug port: Current Control error as twist.")
output_port("ctrl_out", "/base/samples/RigidBodyState").
doc("Controller output as twist")
periodic cycle_time
end
#
# Convert Joint Velocity Commands to Position Commands
#
task_context "ToPosConverter" do
# DEPRECATED use robot_model and robot_model_format instead
property("urdf_file", "/std/string")
# Filename or text representation of the robot model
#
# By default, it is interpreted as a path to a URDF file. If it starts with
# an XML tag, it is interpreted instead as a XML text.
#
# Whether it is a URDF or SDF model is controlled by the robot_model_format
# property
property("robot_model", "/std/string")
# In which format is robot_model
#
# Set to ROBOT_MODEL_URDF for URDF and ROBOT_MODEL_SDF for SDF.
property("robot_model_format", "/kdl_parser/ROBOT_MODEL_FORMAT", :ROBOT_MODEL_URDF)
property("override_output_speed", "double").
doc("Speed to write to joint command")
property("write_speed", "bool").
doc("Write speed value inmput put data or leave it unset")
property("position_scale", "double", 1.0).
doc("position = previous_position + speed * cycle_rate * position_scale")
property("use_position_cmd_as_current", "bool", false).
doc("If true, the converter will use the previous position command as current position.
This may result in less noisy output, than using the real position from joint state")
input_port("joint_status", "/base/samples/Joints").
doc("Current Joint Status")
input_port("command", "/base/commands/Joints").
doc("Velocity based input")
output_port("command_out", "/base/commands/Joints").
doc("Position based output")
port_driven "command"
end