(self, act: jp.ndarray, qp_p: QP, qp_c: QP)
| 13 | |
| 14 | |
| 15 | def apply_reduced(self, act: jp.ndarray, qp_p: QP, qp_c: QP) -> Tuple[P, P]: |
| 16 | axis, angle = self.joint.axis_angle(qp_p, qp_c) |
| 17 | vel = tuple([jp.dot(qp_c.ang - qp_p.ang, ax) for ax in axis]) |
| 18 | axis, angle, vel = jp.array(axis), jp.array(angle), jp.array(vel) |
| 19 | |
| 20 | target_angles = act * jp.pi * 1.2 |
| 21 | |
| 22 | torque = compute_pd_control(target_angles, angle, vel, Kp=self.strength, Kd=self.strength/10) |
| 23 | torque = jp.sum(jp.vmap(jp.multiply)(axis, torque), axis=0) |
| 24 | |
| 25 | dang_p = -self.joint.body_p.inertia * torque |
| 26 | dang_c = self.joint.body_c.inertia * torque |
| 27 | |
| 28 | return dang_p, dang_c |
| 29 | |
| 30 | # def apply_reduced(self, act: jp.ndarray, qp_p: QP, qp_c: QP) -> Tuple[P, P]: |
| 31 | # axis, angle = self.joint.axis_angle(qp_p, qp_c) |
nothing calls this directly
no test coverage detected