def compose_linear_angular(self, linear_vel_cm_s, angular_vel_degrees_s, lin_acc_cm_s_s=None,
ang_acc_degrees_s_s=None):
lin_acc_cm_s_s = WWCommandBody.default_acceleration_linear_cm_s_s \
if lin_acc_cm_s_s is None else lin_acc_cm_s_s
ang_acc_degrees_s_s = WWCommandBody.default_acceleration_angular_degrees_s_s \
if ang_acc_degrees_s_s is None else ang_acc_degrees_s_s
args = {}
args[_rcv.WW_COMMAND_VALUE_LINEAR_VELOCITY_CM_S ] = linear_vel_cm_s
args[_rcv.WW_COMMAND_VALUE_ANGULAR_VELOCITY_DEG_S ] = wwMath.coords_api_to_json_pan(angular_vel_degrees_s)
args[_rcv.WW_COMMAND_VALUE_LINEAR_ACCELERATION_CM_S_S ] = lin_acc_cm_s_s
args[_rcv.WW_COMMAND_VALUE_ANGULAR_ACCELERATION_DEG_S_S] = wwMath.coords_api_to_json_pan(ang_acc_degrees_s_s)
return {_rc.WW_COMMAND_BODY_LINEAR_ANGULAR : args}
def compose_pose(self, x_cm, y_cm, degrees, time, mode, ease, direction, wrap_theta):
x_cm, y_cm = wwMath.coords_api_to_json_pos(x_cm, y_cm)
degrees = wwMath.coords_api_to_json_pan(degrees)
args = {}
args[_rcv.WW_COMMAND_VALUE_AXIS_X ] = x_cm
args[_rcv.WW_COMMAND_VALUE_AXIS_Y ] = y_cm
args[_rcv.WW_COMMAND_VALUE_ANGLE_DEGREE ] = degrees
args[_rcv.WW_COMMAND_VALUE_TIME ] = time
args[_rcv.WW_COMMAND_VALUE_POSE_EASE ] = ease
args[_rcv.WW_COMMAND_VALUE_POSE_MODE ] = mode
args[_rcv.WW_COMMAND_VALUE_POSE_WRAP_THETA] = wrap_theta
args[_rcv.WW_COMMAND_VALUE_POSE_DIRECTION ] = direction
return {_rc.WW_COMMAND_BODY_POSE : args}
def test_coordinate_conversion(self):
d = wwMath.coords_api_to_json_tilt(3.0)
self.assertAlmostEqual(d, -3.0)
d = wwMath.coords_api_to_json_pan(3.0)
self.assertAlmostEqual(d, 3.0)
x, y = wwMath.coords_api_to_json_pos(1.0, 2.0)
self.assertAlmostEqual(x, 2.0)
self.assertAlmostEqual(y, -1.0)
d = wwMath.coords_json_to_api_tilt(3.0)
self.assertAlmostEqual(d, -3.0)
d = wwMath.coords_json_to_api_pan(3.0)
self.assertAlmostEqual(d, 3.0)
x, y = wwMath.coords_json_to_api_pos(1.0, 2.0)
self.assertAlmostEqual(x, -2.0)
self.assertAlmostEqual(y, 1.0)
def parse(self, single_component_dictionary):
if not self.check_fields_exist(single_component_dictionary, _expected_json_fields):
return
x = single_component_dictionary[_rcv.WW_SENSOR_VALUE_AXIS_X ]
y = single_component_dictionary[_rcv.WW_SENSOR_VALUE_AXIS_Y ]
degrees = single_component_dictionary[_rcv.WW_SENSOR_VALUE_ANGLE_DEGREE]
x, y = wwMath.coords_json_to_api_pos(x, y)
degrees = wwMath.coords_api_to_json_pan(degrees)
self._x = x
self._y = y
self._degrees = degrees
if _rcv.WW_SENSOR_VALUE_POSE_WATERMARK in single_component_dictionary:
self._watermark_measured = single_component_dictionary[_rcv.WW_SENSOR_VALUE_POSE_WATERMARK]
self._watermark_inferred = self._watermark_measured
self._valid = True
def stage_pan_voltage(self, pan_voltage_percent):
self._robot.stage_cmds(
self.compose_voltage(
_rc.WW_COMMAND_HEAD_PAN_VOLTAGE,
wwMath.coords_api_to_json_pan(pan_voltage_percent)))
def stage_pan_angle(self, pan_degrees):
self._robot.stage_cmds(
self.compose_angle(_rc.WW_COMMAND_HEAD_POSITION_PAN,
wwMath.coords_api_to_json_pan(pan_degrees)))