-
Notifications
You must be signed in to change notification settings - Fork 69
/
body_part--ebX-jA_B-mc.xml
181 lines (154 loc) · 16.7 KB
/
body_part--ebX-jA_B-mc.xml
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
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
<device xmlns:xi="http://www.w3.org/2001/XInclude" name="body_part-ebX-jA_B-mc" type="embObjMotionControl">
<xi:include href="../../general.xml"/>
<xi:include href="../../hardware/electronics/body_part-ebX-jA_B-eln.xml" />
<xi:include href="../../hardware/mechanicals/body_part-ebX-jA_B-mec.xml" />
<xi:include href="./body_part-ebX-jA_B-mc_service.xml" />
<group name="LIMITS">
<param name="jntPosMax"> Max position of joint (user defined) [expressed in degrees]. It should be minor or equal of hwJntPosMax. See https://wiki.icub.org/wiki/ICub_joints_limits for more details</param>
<param name="jntPosMin"> Min position of joint (user defined) [expressed in degrees]. It should be minor or equal of hwJntPosMin. See https://wiki.icub.org/wiki/ICub_joints_limits for more details</param>
<param name="jntVelMax"> Max velocity of joint [expressed in degrees/second].</param>
<param name="motorNominalCurrents"> It is the current which can be kept for an indefinite amount of time without damaging the motor. The 2FOC uses any value beyond
this threshold in the computation of i2T. It is expressed in [mA]. In datasheet it is found under the name "continuous stall current".
</param>
<param name="motorPeakCurrents"> It is the max current that it can be applied to the motor for a short amount of time. The 2FOC saturates to this value and also
use this value for the integtaion of I2T. It is expressed in [mA]. In datasheet it is found under the name "peak stall current".
</param>
<param name="motorOverloadCurrents"> It is the current which instantaneously damages the motor. It is expressed in [mA].
However, this value is not used by 2FOC FW. Keep it fixed at 21000.
</param>
<param name="motorPwmLimit"> Limit of max PWM which can be applied to the motor. Max value is 32000. This threshold is used after decoupling.
</param>.
</group>
<group name="TIMEOUTS">
<param name="velocity"> timeout on recpetion of velocity setopint by firmware. [expressed in millisec]</param>
</group>
<group name="CONTROLS">
<param name="positionControl"> User name tag of position control. The tag name should be present below. EXAMPLE: MY_PID_POS </param>
<param name="velocityControl"> User name tag of velocity control. The tag name should be present below. If joint can't perform velocity control, use "none". EXAMPLE: MY_PID_VEL </param>
<param name="torqueControl"> User name tag of torque control. The tag name should be present below. If joint can't perform torque control, use "none". EXAMPLE: MY_PID_TRQ </param>
<param name="currentPid"> User name tag of current control. The tag name should be present below. If joint can't perform current control, use "none". EXAMPLE: MY_PID_CUR </param>
</group>
<group name="MY_PID_POS">
<param name="controlLaw"> Specifies the type of algorithm firmware should be performed. Values of this param is:
* Pid_inPos_outPwm: simple pid with posistion setpoint in input and pwm values in output. </param>
* PidPos_withInnerVelPid: position pid with inner velocity pid closed on motor. In this case this group should contain pos pid and vel pid values. See example MY_POS_VEL_CTRL below
<param name="controlUnits"> metric_units or machine_units(raw values) </param>
<param name="pos_kp"> Proportional constant in PID control. [expressed in pwm_raw/degrees (metric_units) or pwm_raw/encoder (machine_units)]</param>
<param name="pos_kd"> Derivative constant in PID control. [expressed in pwm_raw/(degrees/sec) (metric_units) or pwm_raw/(encoder/sec) (machine_units)] </param>
<param name="pos_ki"> Integral constant in PID control. [expressed in pwm_raw/(degrees*sec) (metric_units) or pwm_raw/(encoder*sec) (machine_units)] </param>
<param name="pos_maxOutput"> Limit on PID output [expressed in PWM raw units] </param>
<param name="pos_maxInt"> Anti-windup limit on PID integral part [expressed in PWM raw units] </param>
<param name="pos_shift"> 0 for EMS and MC4+ boards, right shift of kp, kd and ki parameters on boards with integer parameters </param>
<param name="pos_ko"> Openloop output [expressed in PWM raw units]</param>
<param name="pos_stictionUp"> Not used at present </param>
<param name="pos_stictionDwn"> Not used at present </param>
<param name="pos_kff"> Feed forward term constant. The output is proportional to reference. [expressed in pwm_raw/degrees (metric_units) or pwm_raw/encoder (machine_units)]</param>
</group>
<group name="MY_POS_VEL_CTRL">
<param name="controlLaw"> PidPos_withInnerVelPid </param>
<param name="controlUnits"> machine_units </param>
<param name="pos_kp"> Proportional part of the outer PID control loop [expressed in 1/sec] </param>
<param name="pos_kd"> Not used. </param>
<param name="pos_ki"> Not used. </param>
<param name="pos_maxOutput"> Not used (output is limited by max velocity of joint). </param>
<param name="pos_maxInt"> Not used. </param>
<param name="pos_shift"> Not used. </param>
<param name="pos_ko"> Not used. </param>
<param name="pos_stictionUp"> Not used. </param>
<param name="pos_stictionDwn"> Not used. </param>
<param name="pos_kff"> Feed forward velocity term, usually equal to 1 [adimensional] </param>
<param name="vel_kp"> Proportional part of the inner PID control loop [expressed in pwm_raw/(icubdegrees/msec)] </param>
<param name="vel_kd"> Derivative part of the inner PID control loop [expressed in pwm_raw/(icubdegrees/msec^2)] </param>
<param name="vel_ki"> Integral part of the inner PID control loop [expressed in pwm_raw/icubdegrees] </param>
<param name="vel_maxOutput"> Max output of the inner PID control loop [expressed in pwm_raw] </param>
<param name="vel_maxInt"> Anti-windup limit on inner PID integral part [expressed in pwm_raw] </param>
<param name="vel_shift"> Right shift of kp, kd and ki parameters </param>
<param name="vel_ko"> Not used. </param>
<param name="vel_stictionUp"> Not used. </param>
<param name="vel_stictionDwn"> Not used. </param>
<param name="vel_kff"> Not used. </param>
</group>
<group name="MY_PID_TRQ">
<param name="controlLaw"> Pid_inTrq_outPwm </param>
<param name="controlLaw"> Specifies the type of algorithm firmware should be performed. Values of this param is:
* Pid_inTrq_outPwm: simple pid with torue setpoint in input and pwm values in output. </param>
* PidTrq_withInnerVelPid: torque pid with inner velocity pid closed on motor. In this case this group should contain torue pid and vel pid values. See example MY_TRQ_VEL_CTRL below
<param name="controlUnits"> metric_units or machine_units(raw values) </param>
<param name="trq_kp"> Proportional part of the PID control loop [expressed in pwm_raw/Nm] </param>
<param name="trq_kd"> Derivative part of the PID control loop [expressed in pwm_raw/(Nm/sec)] </param>
<param name="trq_ki"> Integral part of the PID control loop [expressed in pwm_raw/(Nm*sec)] </param>
<param name="trq_maxOutput"> Limit on PID output [expressed in PWM raw units] </param>
<param name="trq_maxInt"> Anti-windup limit on PID integral part [expressed in PWM raw units] </param>
<param name="trq_shift"> 0 for EMS and MC4+ boards, right shift of kp, kd and ki parameters on boards with integer parameters </param>
<param name="trq_ko"> Openloop output [expressed in PWM raw units] </param>
<param name="trq_stictionUp"> Not used at present. </param>
<param name="trq_stictionDwn"> Not used at present. </param>
<param name="trq_kff"> Feed forward torque part [expressed in pwm_raw/Nm]</param>
<param name="velocityThres">0 </param> <!-- Velocity threshold for friction compensation [expressed in icubdegrees/sec] -->
<param name="viscousPos"> 0 </param> <!-- Viscous constant used for friction (positive direction ) -->
<param name="viscousNeg"> 0 </param> <!-- Viscous constant used for friction (negative direction ) -->
<param name="coulombPos"> 0 </param> <!-- Coulomb constant used for friction (positive direction ) -->
<param name="coulombNeg"> 0 </param> <!-- Coulomb constant used for friction (negative direction ) -->
<param name="trq_filterType"> Low pass filter on output 0=no filter 1=3 HZ 2=1.1 Hz 3=0.8 Hz 4=0.5 Hz </param>
<param name="trq_ktau"> Gain on Kff part [adimensional] </param>
</group>
<group name="MY_TRQ_VEL_CTRL">
<param name="controlLaw"> Pid_inTrq_outPwm </param>
<param name="controlLaw"> PidTrq_withInnerVelPid </param>
<param name="controlUnits"> metric_units or machine_units(raw values) </param>
<param name="trq_kp"> Torque outer loop proportional part [expressed in (degrees/sec)/Nm] </param>
<param name="trq_kd"> Torque outer loop derivative part [expressed in (degrees/sec)/(Nm/sec)] </param>
<param name="trq_ki"> Torque outer loop itegral part [expressed in (degrees/sec)/(Nm*sec)] </param>
<param name="trq_maxOutput"> Limit on PID output [expressed degrees/sec] </param>
<param name="trq_maxInt"> Anti-windup limit on PID integral part [expressed degrees/sec] </param>
<param name="trq_shift"> 0 for EMS and MC4+ boards, right shift of kp, kd and ki parameters on boards with integer parameters </param>
<param name="trq_ko"> Not used. </param>
<param name="trq_stictionUp"> Not used. </param>
<param name="trq_stictionDwn"> Not used. </param>
<param name="trq_kff"> Not used. </param>
<param name="velocityThres">0 </param> <!-- Velocity threshold for friction compensation [expressed in icubdegrees/sec] -->
<param name="viscousPos"> 0 </param> <!-- Viscous constant used for friction (positive direction ) -->
<param name="viscousNeg"> 0 </param> <!-- Viscous constant used for friction (negative direction ) -->
<param name="coulombPos"> 0 </param> <!-- Coulomb constant used for friction (positive direction ) -->
<param name="coulombNeg"> 0 </param> <!-- Coulomb constant used for friction (negative direction ) -->
<param name="trq_filterType"> Not used. </param>
<param name="trq_ktau"> Not used. </param>
<param name="vel_kp"> Velocity inner loop proportional part constant [expressed in pwm_raw/(icubdegrees/msec)] </param>
<param name="vel_kd"> Velocity inner loop derivative part constant [expressed in pwm_raw/(icubdegrees/msec^2)] </param>
<param name="vel_ki"> Velocity inner loop integral part constant [expressed in pwm_raw/icubdegrees/msec] </param>
<param name="vel_maxOutput"> Limit on PID output [expressed icubdegrees/msec] </param>
<param name="vel_maxInt"> Anti-windup limit on PID integral part [expressed in icubegrees/msec] </param>
<param name="vel_shift"> Right shift of kp, kd and ki parameters </param>
<param name="vel_ko"> Not used. </param>
<param name="vel_stictionUp"> Not used. </param>
<param name="vel_stictionDwn"> Not used. </param>
<param name="vel_kff"> Not used. </param>
</group>
<group name="IMPEDANCE">
<param name="stiffness"> stiffness values. If torque control pid has metric_units, measurament units is [Nm/degree] else raw value </param>
<param name="damping"> damping values. If torque control pid has metric_units, measurament units is [Nm/(degree/sec)] else raw value </param>
</group>
<group name="MY_PID_CUR">
<param name="controlLaw"> At present, not a real current pid is implemented, but a simple feedback control.
So the only value of this parameter is: limitscurrent </param>
<param name="controlUnits"> machine_units </param>
<param name="cur_kp"> Current control loop proportional part [expressed in pwm_raw/mA] </param>
<param name="cur_kd"> Current control loop derivative part [expressed in pwm_raw/(mA/sec)] </param>
<param name="cur_ki"> Current control loop integral part [expressed in pwm_raw/(mA*sec)] </param>
<param name="cur_shift"> right shift of kp, kd and ki parameters </param>
<param name="cur_maxOutput"> Limit on PID output [expressed in PWM raw units] </param>
<param name="cur_maxInt"> Anti-windup limit on PID integral part [expressed in PWM raw units] </param>
<param name="cur_ko"> Not used. </param>
<param name="cur_stictionUp"> Not used. </param>
<param name="cur_stictionDwn"> Not used. </param>
<param name="cur_kff"> Not used. </param>
</group>
<group name="OTHER_CONTROL_PARAMETERS">
<param name="deadZone"> the maximum precision in reaching the target position. This is an optional paramter, therefore if is not specified the firmware use the following ones:
- for joints with aea encoder it is 0.0495 degree
- for joints with amo encoder it is 0.0049 degree
</param>
</group>
</device>