def DoCalcAbstractOutput(self, context, y_data):
"""
Sets external forces
"""
external_forces = []
body_positions = self.body_positions_input_port.Eval(context)
for arm in self.arm_info:
desired = self.desired_input_ports[arm].Eval(context)
pose = body_positions[int(self.arm_info[arm])]
rotation = RollPitchYaw(pose.rotation())
estimated = [
rotation.roll_angle(),
rotation.pitch_angle(),
rotation.yaw_angle()
]
estimated.extend(pose.translation())
error = np.subtract(desired, estimated)
error[0:3] = self.clamp_angles(error[0:3])
prev_error = context.get_mutable_discrete_state(
self.previous_error[arm])
integral = context.get_mutable_discrete_state(self.integral[arm])
integral.SetFromVector(integral.get_value() +
error * self.timestep)
derivative = np.subtract(error,
prev_error.get_value()) / self.timestep
forces = np.multiply(self.kp, error)
forces += np.multiply(self.kd, derivative)
forces += np.multiply(self.ki, integral.get_value())
clipped_forces = np.clip(
forces, [-100, -100, -100, -10000, -10000, -10000],
[100, 100, 100, 10000, 10000, 10000])
prev_error.SetFromVector(error)
# print(f"arm {arm} forces {clipped_forces}")
external_forces.append(
self.get_external_force(self.arm_info[arm], clipped_forces))
y_data.set_value(external_forces)
def pose_to_value(pose):
rpy = RollPitchYaw(pose.rotation())
xyz = pose.translation()
return PoseSliders.Value(rpy.roll_angle(), rpy.pitch_angle(),
rpy.yaw_angle(), xyz[0], xyz[1], xyz[2])
