Add <damper> actuator shortcut and related mjGAIN_AFFINE.

**Rationale**

A general `mjGAIN_AFFINE` actuator adds the term `force = (c + kp * length + kv * velocity) * ctrl`. The damper shortcut restricts it to `kv * velocity * ctrl` and sets the `ctrllimited` attribute to true.

**API**

```xml
<actuator>
  <damper ctrlrange="0 1" kv="1"/>
</actuator>
```

* Required attributes: `ctrlrange` (>=0)
* Optional attributes: `kv` (>=0)

PiperOrigin-RevId: 455366866
Change-Id: I88773abbeccabb6442f215c9fc05e205df63f692
This commit is contained in:
Alessio Quaglino
2022-06-16 06:20:49 -07:00
committed by Copybara-Service
parent e0b6ba4a13
commit 307cf692e5
14 changed files with 283 additions and 34 deletions
+18 -10
View File
@@ -735,33 +735,41 @@ static void mjd_actuator_vel(const mjModel* m, mjData* d, mjtNum* DfDv) {
// process actuators
for (int i=0; i<m->nu; i++) {
mjtNum bias_vel = 0, gain_vel = 0;
// affine bias
if (m->actuator_biastype[i]==mjBIAS_AFFINE) {
// extract bias info: prm = [const, kp, kv]
mjtNum* prm = m->actuator_biasprm + mjNBIAS*i;
bias_vel = (m->actuator_biasprm + mjNBIAS*i)[2];
}
// add
mjtNum B = prm[2];
addJTBJ(DfDv, d->actuator_moment+i*nv, &B, 1, nv);
// affine gain
if (m->actuator_gaintype[i]==mjGAIN_AFFINE) {
// extract bias info: prm = [const, kp, kv]
gain_vel = (m->actuator_gainprm + mjNGAIN*i)[2];
}
// muscle gain
else if (m->actuator_gaintype[i]==mjGAIN_MUSCLE) {
mjtNum B = mjd_muscleGain_vel(d->actuator_length[i],
gain_vel = mjd_muscleGain_vel(d->actuator_length[i],
d->actuator_velocity[i],
m->actuator_lengthrange+2*i,
m->actuator_acc0[i],
m->actuator_gainprm + mjNGAIN*i);
}
// force = gain .* [ctrl/act]
// force = gain .* [ctrl/act]
if (gain_vel!=0) {
if (m->actuator_dyntype[i]==mjDYN_NONE) {
B *= d->ctrl[i];
bias_vel += gain_vel * d->ctrl[i];
} else {
B *= d->act[i-(m->nu - m->na)];
bias_vel += gain_vel * d->act[i-(m->nu - m->na)];
}
}
// add
addJTBJ(DfDv, d->actuator_moment+i*nv, &B, 1, nv);
// add
if (bias_vel!=0) {
addJTBJ(DfDv, d->actuator_moment+i*nv, &bias_vel, 1, nv);
}
}
}
+4
View File
@@ -197,6 +197,10 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
gain = prm[0];
break;
case mjGAIN_AFFINE: // affine: prm = [const, kp, kv]
gain = prm[0] + prm[1]*d->actuator_length[i] + prm[2]*d->actuator_velocity[i];
break;
case mjGAIN_MUSCLE: // muscle gain
gain = mju_muscleGain(d->actuator_length[i],
d->actuator_velocity[i],