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:
committed by
Copybara-Service
parent
e0b6ba4a13
commit
307cf692e5
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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],
|
||||
|
||||
Reference in New Issue
Block a user