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])