def drive(self, left_throttle, right_throttle):
front_right_update_obj = at.UrdfBotControlledMotionComponentControlSignal(component_name=self.fr_joint_name, control_signal_values={'Value': right_throttle})
front_left_update_obj = at.UrdfBotControlledMotionComponentControlSignal(component_name=self.fl_joint_name, control_signal_values={'Value': left_throttle})
back_right_update_obj = at.UrdfBotControlledMotionComponentControlSignal(component_name=self.br_joint_name, control_signal_values={'Value': right_throttle})
back_left_update_obj = at.UrdfBotControlledMotionComponentControlSignal(component_name=self.bl_joint_name, control_signal_values={'Value': left_throttle})
self.updateControlledMotionComponentControlSignal(front_right_update_obj)
self.updateControlledMotionComponentControlSignal(front_left_update_obj)
self.updateControlledMotionComponentControlSignal(back_right_update_obj)
self.updateControlledMotionComponentControlSignal(back_left_update_obj)
def set_arm_pose(self, platform_signal, first_signal, second_signal,
third_signal, fourth_signal):
if (platform_signal < 0 or platform_signal > 1):
raise ValueError(
'Platform_signal is {0}, which is outside the expected range of [0, 1]'
.format(platform_signal))
if (first_signal < 0 or first_signal > 1):
raise ValueError(
'First_signal is {0}, which is outside the expected range of [0, 1]'
.format(first_signal))
if (second_signal < 0 or second_signal > 1):
raise ValueError(
'Second_signal is {0}, which is outside the expected range of [0, 1]'
.format(second_signal))
if (third_signal < 0 or third_signal > 1):
raise ValueError(
'Third_signal is {0}, which is outside the expected range of [0, 1]'
.format(third_signal))
if (fourth_signal < 0 or fourth_signal > 1):
raise ValueError(
'Fourth_signal is {0}, which is outside the expected range of [0, 1]'
.format(fourth_signal))
platform_update_obj = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.platform_joint_name,
control_signal_values={'Value': platform_signal})
first_update_obj = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.first_joint_name,
control_signal_values={'Value': first_signal})
second_update_obj = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.second_joint_name,
control_signal_values={'Value': second_signal})
third_update_obj = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.third_joint_name,
control_signal_values={'Value': third_signal})
fourth_update_obj = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.fourth_joint_name,
control_signal_values={'Value': fourth_signal})
self.airsim_client.updateControlledMotionComponentControlSignal(
platform_update_obj)
self.airsim_client.updateControlledMotionComponentControlSignal(
first_update_obj)
self.airsim_client.updateControlledMotionComponentControlSignal(
second_update_obj)
self.airsim_client.updateControlledMotionComponentControlSignal(
third_update_obj)
self.airsim_client.updateControlledMotionComponentControlSignal(
fourth_update_obj)
def set_manipulator(self, magnitude):
if (magnitude < 0 or magnitude > 1):
raise ValueError(
'Magnitude is {0}, which is outside the range [0, 1]'.format(
magnitude))
left_update_obj = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.manipulator_left_joint_name,
control_signal_values={'Value': magnitude})
right_update_obj = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.manipulator_right_joint_name,
control_signal_values={'Value': magnitude})
self.airsim_client.updateControlledMotionComponentControlSignal(
left_update_obj)
self.airsim_client.updateControlledMotionComponentControlSignal(
right_update_obj)
def drive(self, magnitude, theta):
if (abs(magnitude) > 1):
raise ValueError(
'Speed must be between 0 and 1, provided value {0}'.format(
magnitude))
left_multiplier = self.drive_angle_multiplier(theta)
right_multiplier = self.drive_angle_multiplier(theta - (self.pi / 2))
right_update_obj = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.right_wheel_joint_name,
control_signal_values={'Value': magnitude * right_multiplier})
left_update_obj = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.left_wheel_joint_name,
control_signal_values={'Value': magnitude * left_multiplier})
self.airsim_client.updateControlledMotionComponentControlSignal(
right_update_obj)
self.airsim_client.updateControlledMotionComponentControlSignal(
left_update_obj)
def set_servo(self, control_signal_value):
if (control_signal_value < 0 or control_signal_value > 1):
raise ValueError(
'control_signal_value = {0}, which is outside the range [0, 1]'
.format(control_signal_valuew))
update_value = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.__servo_name,
control_signal_values={'Value': control_signal_value})
self.airsim_client.updateControlledMotionComponentControlSignal(
update_value, vehicle_name='')
def diff_drive(self, linear, angular):
left_velocity = ((2 * linear) -
(angular * self.wheel_base)) / (2 * self.wheel_radius)
right_velocity = (
(2 * linear) +
(angular * self.wheel_base)) / (2 * self.wheel_radius)
left_multiplier = self.range_map(left_velocity, -self.max_speed,
self.max_speed)
right_multiplier = self.range_map(right_velocity, -self.max_speed,
self.max_speed)
right_update_obj = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.right_wheel_joint_name,
control_signal_values={'Value': right_multiplier})
left_update_obj = at.UrdfBotControlledMotionComponentControlSignal(
component_name=self.left_wheel_joint_name,
control_signal_values={'Value': left_multiplier})
self.airsim_client.updateControlledMotionComponentControlSignal(
right_update_obj)
self.airsim_client.updateControlledMotionComponentControlSignal(
left_update_obj)