class A1Robot(a1.A1):
"""Interface for real A1 robot."""
MPC_BODY_MASS = 14.2
MPC_BODY_INERTIA = (0.07335, 0, 0, 0, 0.25068, 0, 0, 0, 0.25447)
MPC_BODY_HEIGHT = 0.24
ACTION_CONFIG = [
locomotion_gym_config.ScalarField(name="FR_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="FR_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="FR_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
locomotion_gym_config.ScalarField(name="FL_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="FL_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="FL_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
locomotion_gym_config.ScalarField(name="RR_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="RR_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="RR_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
locomotion_gym_config.ScalarField(name="RL_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="RL_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="RL_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
]
def __init__(self, pybullet_client, time_step=0.002, **kwargs):
"""Initializes the robot class."""
# Initialize pd gain vector
self.motor_kps = np.array([ABDUCTION_P_GAIN, HIP_P_GAIN, KNEE_P_GAIN] * 4)
self.motor_kds = np.array([ABDUCTION_D_GAIN, HIP_D_GAIN, KNEE_D_GAIN] * 4)
self._pybullet_client = pybullet_client
self.time_step = time_step
# Robot state variables
self._init_complete = False
self._base_orientation = None
self._raw_state = None
self._last_raw_state = None
self._motor_angles = np.zeros(12)
self._motor_velocities = np.zeros(12)
self._joint_states = None
self._last_reset_time = time.time()
self._velocity_estimator = a1_robot_velocity_estimator.VelocityEstimator(
self)
# Initiate LCM channel for robot state and actions
self.lc = lcm.LCM()
self._command_channel_name = COMMAND_CHANNEL_NAME
# Send dummy command so that the robot starts responding
command = comm.LowCmd()
command.levelFlag = 0xff # pylint: disable=C0103
self.lc.publish(self._command_channel_name, command)
self._state_channel_name = STATE_CHANNEL_NAME
self._state_channel = self.lc.subscribe(STATE_CHANNEL_NAME,
self.ReceiveObservationAsync)
self._is_alive = True
self.subscribe_thread = threading.Thread(target=self._LCMSubscribeLoop,
args=())
self.subscribe_thread.start()
while self._last_raw_state is None:
logging.info("Robot sensor reading not ready yet, sleep for 1 second...")
time.sleep(1)
kwargs['on_rack'] = True
super(A1Robot, self).__init__(pybullet_client,
time_step=time_step,
**kwargs)
self._init_complete = True
def _LCMSubscribeLoop(self):
while self._is_alive:
self.lc.handle_timeout(100)
def ReceiveObservation(self):
"""Receives observation from robot.
Synchronous ReceiveObservation is not supported in A1,
so changging it to noop instead.
"""
pass
def ReceiveObservationAsync(self, channel, data):
"""Receive the observation from sensors.
This function is called once per step. The observations are only updated
when this function is called.
"""
del channel # unused
stream = BytesIO(data)
state = comm.LowState()
stream.readinto(state) # pytype: disable=wrong-arg-types
self._last_raw_state = self._raw_state
self._raw_state = state
# Convert quaternion from wxyz to xyzw, which is default for Pybullet.
q = state.imu.quaternion
self._base_orientation = np.array([q[1], q[2], q[3], q[0]])
self._motor_angles = np.array([motor.q for motor in state.motorState[:12]])
self._motor_velocities = np.array(
[motor.dq for motor in state.motorState[:12]])
self._joint_states = np.array(
list(zip(self._motor_angles, self._motor_velocities)))
if self._init_complete:
self._SetMotorAnglesInSim(self._motor_angles, self._motor_velocities)
self._velocity_estimator.update(self._raw_state)
def _SetMotorAnglesInSim(self, motor_angles, motor_velocities):
for i, motor_id in enumerate(self._motor_id_list):
self._pybullet_client.resetJointState(self.quadruped, motor_id,
motor_angles[i],
motor_velocities[i])
def GetTrueMotorAngles(self):
return self._motor_angles
def GetMotorAngles(self):
return minitaur.MapToMinusPiToPi(self._motor_angles)
def GetMotorVelocities(self):
return self._motor_velocities
def GetBasePosition(self):
return self._pybullet_client.getBasePositionAndOrientation(
self.quadruped)[0]
def GetBaseRollPitchYaw(self):
return self._pybullet_client.getEulerFromQuaternion(self._base_orientation)
def GetTrueBaseRollPitchYaw(self):
return self._pybullet_client.getEulerFromQuaternion(self._base_orientation)
def GetBaseRollPitchYawRate(self):
return self.GetTrueBaseRollPitchYawRate()
def GetTrueBaseRollPitchYawRate(self):
return np.array(self._raw_state.imu.gyroscope)
def GetBaseVelocity(self):
return self._velocity_estimator.estimated_velocity
def GetFootContacts(self):
return np.array(self._raw_state.footForce) > 20
def GetTimeSinceReset(self):
return time.time() - self._last_reset_time
@property
def motor_velocities(self):
return self._motor_velocities
def ApplyAction(self, motor_commands, motor_control_mode=None):
"""Clips and then apply the motor commands using the motor model.
Args:
motor_commands: np.array. Can be motor angles, torques, hybrid commands,
or motor pwms (for Minitaur only).
motor_control_mode: A MotorControlMode enum.
"""
if motor_control_mode is None:
motor_control_mode = self._motor_control_mode
command = comm.LowCmd()
command.levelFlag = 0xff #pylint:disable=invalid-name
if motor_control_mode == robot_config.MotorControlMode.POSITION:
for motor_id in range(NUM_MOTORS):
command.motorCmd[motor_id].mode = 0x0A
command.motorCmd[motor_id].q = motor_commands[motor_id]
command.motorCmd[motor_id].Kp = self.motor_kps[motor_id]
command.motorCmd[motor_id].dq = 0
command.motorCmd[motor_id].Kd = self.motor_kds[motor_id]
command.motorCmd[motor_id].tau = 0
# Gravity compensation
command.motorCmd[0].tau = -0.65
command.motorCmd[3].tau = 0.65
command.motorCmd[6].tau = -0.65
command.motorCmd[9].tau = 0.65
elif motor_control_mode == robot_config.MotorControlMode.TORQUE:
for motor_id in range(NUM_MOTORS):
command.motorCmd[motor_id].mode = 0x0A
command.motorCmd[motor_id].q = 0
command.motorCmd[motor_id].Kp = 0
command.motorCmd[motor_id].dq = 0
command.motorCmd[motor_id].Kd = 0
command.motorCmd[motor_id].tau = motor_commands[motor_id]
elif motor_control_mode == robot_config.MotorControlMode.HYBRID:
for motor_id in range(NUM_MOTORS):
command.motorCmd[motor_id].mode = 0x0A
command.motorCmd[motor_id].q = motor_commands[motor_id * 5]
command.motorCmd[motor_id].Kp = motor_commands[motor_id * 5 + 1]
command.motorCmd[motor_id].dq = motor_commands[motor_id * 5 + 2]
command.motorCmd[motor_id].Kd = motor_commands[motor_id * 5 + 3]
command.motorCmd[motor_id].tau = motor_commands[motor_id * 5 + 4]
else:
raise ValueError('Unknown motor control mode for A1 robot: {}.'.format(
motor_control_mode))
self.lc.publish(self._command_channel_name, command)
def Reset(self, reload_urdf=True, default_motor_angles=None, reset_time=3.0):
"""Reset the robot to default motor angles."""
super(A1Robot, self).Reset(reload_urdf=reload_urdf,
default_motor_angles=default_motor_angles,
reset_time=-1)
logging.warning(
"About to reset the robot, make sure the robot is hang-up.")
if not default_motor_angles:
default_motor_angles = a1.INIT_MOTOR_ANGLES
current_motor_angles = self.GetMotorAngles()
for t in np.arange(0, reset_time, self.time_step * self._action_repeat):
blend_ratio = t / reset_time
action = blend_ratio * default_motor_angles + (
1 - blend_ratio) * current_motor_angles
self.ApplyAction(action, robot_config.MotorControlMode.POSITION)
time.sleep(self.time_step * self._action_repeat)
if self._enable_action_filter:
self._ResetActionFilter()
self._velocity_estimator.reset()
self._state_action_counter = 0
self._step_counter = 0
self._last_reset_time = time.time()
def Terminate(self):
self._is_alive = False
class Laikago(minitaur.Minitaur):
"""A simulation for the Laikago robot."""
MPC_BODY_MASS = 215 / 9.8
MPC_BODY_INERTIA = (0.07335, 0, 0, 0, 0.25068, 0, 0, 0, 0.25447)
MPC_BODY_HEIGHT = 0.42
ACTION_CONFIG = [
locomotion_gym_config.ScalarField(name="motor_angle_0",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND),
locomotion_gym_config.ScalarField(name="motor_angle_1",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND),
locomotion_gym_config.ScalarField(name="motor_angle_2",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND),
locomotion_gym_config.ScalarField(name="motor_angle_3",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND),
locomotion_gym_config.ScalarField(name="motor_angle_4",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND),
locomotion_gym_config.ScalarField(name="motor_angle_5",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND),
locomotion_gym_config.ScalarField(name="motor_angle_6",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND),
locomotion_gym_config.ScalarField(name="motor_angle_7",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND),
locomotion_gym_config.ScalarField(name="motor_angle_8",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND),
locomotion_gym_config.ScalarField(name="motor_angle_9",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND),
locomotion_gym_config.ScalarField(name="motor_angle_10",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND),
locomotion_gym_config.ScalarField(name="motor_angle_11",
upper_bound=UPPER_BOUND,
lower_bound=LOWER_BOUND)
]
def __init__(
self,
pybullet_client,
motor_control_mode,
urdf_filename=URDF_FILENAME,
enable_clip_motor_commands=False,
time_step=0.001,
action_repeat=33,
sensors=None,
control_latency=0.002,
on_rack=False,
enable_action_interpolation=True,
enable_action_filter=False,
reset_time=-1,
allow_knee_contact=False,
):
self._urdf_filename = urdf_filename
self._allow_knee_contact = allow_knee_contact
self._enable_clip_motor_commands = enable_clip_motor_commands
motor_kp = [
ABDUCTION_P_GAIN, HIP_P_GAIN, KNEE_P_GAIN, ABDUCTION_P_GAIN,
HIP_P_GAIN, KNEE_P_GAIN, ABDUCTION_P_GAIN, HIP_P_GAIN, KNEE_P_GAIN,
ABDUCTION_P_GAIN, HIP_P_GAIN, KNEE_P_GAIN
]
motor_kd = [
ABDUCTION_D_GAIN, HIP_D_GAIN, KNEE_D_GAIN, ABDUCTION_D_GAIN,
HIP_D_GAIN, KNEE_D_GAIN, ABDUCTION_D_GAIN, HIP_D_GAIN, KNEE_D_GAIN,
ABDUCTION_D_GAIN, HIP_D_GAIN, KNEE_D_GAIN
]
super(Laikago, self).__init__(
pybullet_client=pybullet_client,
time_step=time_step,
action_repeat=action_repeat,
num_motors=NUM_MOTORS,
dofs_per_leg=DOFS_PER_LEG,
motor_direction=JOINT_DIRECTIONS,
motor_offset=JOINT_OFFSETS,
motor_overheat_protection=False,
motor_control_mode=motor_control_mode,
motor_model_class=laikago_motor.LaikagoMotorModel,
sensors=sensors,
motor_kp=motor_kp,
motor_kd=motor_kd,
control_latency=control_latency,
on_rack=on_rack,
enable_action_interpolation=enable_action_interpolation,
enable_action_filter=enable_action_filter,
reset_time=reset_time)
def _LoadRobotURDF(self):
laikago_urdf_path = self.GetURDFFile()
if self._self_collision_enabled:
self.quadruped = self._pybullet_client.loadURDF(
laikago_urdf_path,
self._GetDefaultInitPosition(),
self._GetDefaultInitOrientation(),
flags=self._pybullet_client.URDF_USE_SELF_COLLISION)
else:
self.quadruped = self._pybullet_client.loadURDF(
laikago_urdf_path, self._GetDefaultInitPosition(),
self._GetDefaultInitOrientation())
def _SettleDownForReset(self, default_motor_angles, reset_time):
self.ReceiveObservation()
if reset_time <= 0:
return
for _ in range(500):
self._StepInternal(
INIT_MOTOR_ANGLES,
motor_control_mode=robot_config.MotorControlMode.POSITION)
if default_motor_angles is not None:
num_steps_to_reset = int(reset_time / self.time_step)
for _ in range(num_steps_to_reset):
self._StepInternal(
default_motor_angles,
motor_control_mode=robot_config.MotorControlMode.POSITION)
def GetHipPositionsInBaseFrame(self):
return _DEFAULT_HIP_POSITIONS
def GetFootContacts(self):
all_contacts = self._pybullet_client.getContactPoints(
bodyA=self.quadruped)
contacts = [False, False, False, False]
for contact in all_contacts:
# Ignore self contacts
if contact[_BODY_B_FIELD_NUMBER] == self.quadruped:
continue
try:
toe_link_index = self._foot_link_ids.index(
contact[_LINK_A_FIELD_NUMBER])
contacts[toe_link_index] = True
except ValueError:
continue
return contacts
def ComputeJacobian(self, leg_id):
"""Compute the Jacobian for a given leg."""
# Because of the default rotation in the Laikago URDF, we need to reorder
# the rows in the Jacobian matrix.
return super(Laikago, self).ComputeJacobian(leg_id)[(2, 0, 1), :]
def ResetPose(self, add_constraint):
del add_constraint
for name in self._joint_name_to_id:
joint_id = self._joint_name_to_id[name]
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=(joint_id),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
for name, i in zip(MOTOR_NAMES, range(len(MOTOR_NAMES))):
if "hip_motor_2_chassis_joint" in name:
angle = INIT_MOTOR_ANGLES[i] + HIP_JOINT_OFFSET
elif "upper_leg_2_hip_motor_joint" in name:
angle = INIT_MOTOR_ANGLES[i] + UPPER_LEG_JOINT_OFFSET
elif "lower_leg_2_upper_leg_joint" in name:
angle = INIT_MOTOR_ANGLES[i] + KNEE_JOINT_OFFSET
else:
raise ValueError(
"The name %s is not recognized as a motor joint." % name)
self._pybullet_client.resetJointState(self.quadruped,
self._joint_name_to_id[name],
angle,
targetVelocity=0)
def GetURDFFile(self):
return self._urdf_filename
def _BuildUrdfIds(self):
"""Build the link Ids from its name in the URDF file.
Raises:
ValueError: Unknown category of the joint name.
"""
num_joints = self._pybullet_client.getNumJoints(self.quadruped)
self._chassis_link_ids = [-1]
self._leg_link_ids = []
self._motor_link_ids = []
self._knee_link_ids = []
self._foot_link_ids = []
for i in range(num_joints):
joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
joint_name = joint_info[1].decode("UTF-8")
joint_id = self._joint_name_to_id[joint_name]
if _CHASSIS_NAME_PATTERN.match(joint_name):
self._chassis_link_ids.append(joint_id)
elif _MOTOR_NAME_PATTERN.match(joint_name):
self._motor_link_ids.append(joint_id)
# We either treat the lower leg or the toe as the foot link, depending on
# the urdf version used.
elif _KNEE_NAME_PATTERN.match(joint_name):
self._knee_link_ids.append(joint_id)
elif _TOE_NAME_PATTERN.match(joint_name):
self._foot_link_ids.append(joint_id)
else:
raise ValueError("Unknown category of joint %s" % joint_name)
self._leg_link_ids.extend(self._knee_link_ids)
self._leg_link_ids.extend(self._foot_link_ids)
if self._allow_knee_contact:
self._foot_link_ids.extend(self._knee_link_ids)
self._chassis_link_ids.sort()
self._motor_link_ids.sort()
self._foot_link_ids.sort()
self._leg_link_ids.sort()
def _GetMotorNames(self):
return MOTOR_NAMES
def _GetDefaultInitPosition(self):
if self._on_rack:
return INIT_RACK_POSITION
else:
return INIT_POSITION
def _GetDefaultInitOrientation(self):
# The Laikago URDF assumes the initial pose of heading towards z axis,
# and belly towards y axis. The following transformation is to transform
# the Laikago initial orientation to our commonly used orientation: heading
# towards -x direction, and z axis is the up direction.
init_orientation = pyb.getQuaternionFromEuler(
[math.pi / 2.0, 0, math.pi / 2.0])
return init_orientation
def GetDefaultInitPosition(self):
"""Get default initial base position."""
return self._GetDefaultInitPosition()
def GetDefaultInitOrientation(self):
"""Get default initial base orientation."""
return self._GetDefaultInitOrientation()
def GetDefaultInitJointPose(self):
"""Get default initial joint pose."""
joint_pose = (INIT_MOTOR_ANGLES + JOINT_OFFSETS) * JOINT_DIRECTIONS
return joint_pose
def ApplyAction(self, motor_commands, motor_control_mode):
"""Clips and then apply the motor commands using the motor model.
Args:
motor_commands: np.array. Can be motor angles, torques, hybrid commands,
or motor pwms (for Minitaur only).N
motor_control_mode: A MotorControlMode enum.
"""
if self._enable_clip_motor_commands:
motor_commands = self._ClipMotorCommands(motor_commands)
super(Laikago, self).ApplyAction(motor_commands, motor_control_mode)
def _ClipMotorCommands(self, motor_commands):
"""Clips motor commands.
Args:
motor_commands: np.array. Can be motor angles, torques, hybrid commands,
or motor pwms (for Minitaur only).
Returns:
Clipped motor commands.
"""
# clamp the motor command by the joint limit, in case weired things happens
max_angle_change = MAX_MOTOR_ANGLE_CHANGE_PER_STEP
current_motor_angles = self.GetMotorAngles()
motor_commands = np.clip(motor_commands,
current_motor_angles - max_angle_change,
current_motor_angles + max_angle_change)
return motor_commands
@classmethod
def GetConstants(cls):
del cls
return laikago_constants
class A1Robot(a1.A1):
"""Interface for real A1 robot."""
MPC_BODY_MASS = 108 / 9.8
MPC_BODY_INERTIA = np.array((0.24, 0, 0, 0, 0.80, 0, 0, 0, 1.00))
MPC_BODY_HEIGHT = 0.24
ACTION_CONFIG = [
locomotion_gym_config.ScalarField(name="FR_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="FR_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="FR_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
locomotion_gym_config.ScalarField(name="FL_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="FL_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="FL_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
locomotion_gym_config.ScalarField(name="RR_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="RR_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="RR_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
locomotion_gym_config.ScalarField(name="RL_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="RL_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="RL_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
]
def __init__(self, pybullet_client, time_step=0.002, **kwargs):
"""Initializes the robot class."""
# Initialize pd gain vector
self.motor_kps = np.array([ABDUCTION_P_GAIN, HIP_P_GAIN, KNEE_P_GAIN] *
4)
self.motor_kds = np.array([ABDUCTION_D_GAIN, HIP_D_GAIN, KNEE_D_GAIN] *
4)
self._pybullet_client = pybullet_client
self.time_step = time_step
# Robot state variables
self._init_complete = False
self._base_orientation = None
self._raw_state = None
self._last_raw_state = None
self._motor_angles = np.zeros(12)
self._motor_velocities = np.zeros(12)
self._joint_states = None
self._last_reset_time = time.time()
self._velocity_estimator = a1_robot_velocity_estimator.VelocityEstimator(
self)
# Initiate UDP for robot state and actions
self._robot_interface = RobotInterface()
self._robot_interface.send_command(np.zeros(60, dtype=np.float32))
kwargs['on_rack'] = True
super(A1Robot, self).__init__(pybullet_client,
time_step=time_step,
**kwargs)
self._init_complete = True
def ReceiveObservation(self):
"""Receives observation from robot.
Synchronous ReceiveObservation is not supported in A1,
so changging it to noop instead.
"""
state = self._robot_interface.receive_observation()
self._raw_state = state
# Convert quaternion from wxyz to xyzw, which is default for Pybullet.
q = state.imu.quaternion
self._base_orientation = np.array([q[1], q[2], q[3], q[0]])
self._motor_angles = np.array(
[motor.q for motor in state.motorState[:12]])
self._motor_velocities = np.array(
[motor.dq for motor in state.motorState[:12]])
self._joint_states = np.array(
list(zip(self._motor_angles, self._motor_velocities)))
if self._init_complete:
# self._SetRobotStateInSim(self._motor_angles, self._motor_velocities)
self._velocity_estimator.update(self._raw_state)
def _SetRobotStateInSim(self, motor_angles, motor_velocities):
self._pybullet_client.resetBasePositionAndOrientation(
self.quadruped, self.GetBasePosition(), self.GetBaseOrientation())
for i, motor_id in enumerate(self._motor_id_list):
self._pybullet_client.resetJointState(self.quadruped, motor_id,
motor_angles[i],
motor_velocities[i])
def GetTrueMotorAngles(self):
return self._motor_angles.copy()
def GetMotorAngles(self):
return minitaur.MapToMinusPiToPi(self._motor_angles).copy()
def GetMotorVelocities(self):
return self._motor_velocities.copy()
def GetBasePosition(self):
return self._pybullet_client.getBasePositionAndOrientation(
self.quadruped)[0]
def GetBaseRollPitchYaw(self):
return self._pybullet_client.getEulerFromQuaternion(
self._base_orientation)
def GetTrueBaseRollPitchYaw(self):
return self._pybullet_client.getEulerFromQuaternion(
self._base_orientation)
def GetBaseRollPitchYawRate(self):
return self.GetTrueBaseRollPitchYawRate()
def GetTrueBaseRollPitchYawRate(self):
return np.array(self._raw_state.imu.gyroscope).copy()
def GetBaseVelocity(self):
return self._velocity_estimator.estimated_velocity.copy()
def GetFootContacts(self):
return np.array(self._raw_state.footForce) > 20
def GetTimeSinceReset(self):
return time.time() - self._last_reset_time
def GetBaseOrientation(self):
return self._base_orientation.copy()
@property
def motor_velocities(self):
return self._motor_velocities.copy()
def ApplyAction(self, motor_commands, motor_control_mode=None):
"""Clips and then apply the motor commands using the motor model.
Args:
motor_commands: np.array. Can be motor angles, torques, hybrid commands,
or motor pwms (for Minitaur only).
motor_control_mode: A MotorControlMode enum.
"""
if motor_control_mode is None:
motor_control_mode = self._motor_control_mode
command = np.zeros(60, dtype=np.float32)
if motor_control_mode == robot_config.MotorControlMode.POSITION:
for motor_id in range(NUM_MOTORS):
command[motor_id * 5] = motor_commands[motor_id]
command[motor_id * 5 + 1] = self.motor_kps[motor_id]
command[motor_id * 5 + 3] = self.motor_kds[motor_id]
elif motor_control_mode == robot_config.MotorControlMode.TORQUE:
for motor_id in range(NUM_MOTORS):
command[motor_id * 5 + 4] = motor_commands[motor_id]
elif motor_control_mode == robot_config.MotorControlMode.HYBRID:
command = np.array(motor_commands, dtype=np.float32)
else:
raise ValueError(
'Unknown motor control mode for A1 robot: {}.'.format(
motor_control_mode))
self._robot_interface.send_command(command)
def Reset(self,
reload_urdf=True,
default_motor_angles=None,
reset_time=3.0):
"""Reset the robot to default motor angles."""
super(A1Robot, self).Reset(reload_urdf=reload_urdf,
default_motor_angles=default_motor_angles,
reset_time=-1)
logging.warning(
"About to reset the robot, make sure the robot is hang-up.")
if not default_motor_angles:
default_motor_angles = a1.INIT_MOTOR_ANGLES
current_motor_angles = self.GetMotorAngles()
# Stand up in 1.5 seconds, and keep the behavior in this way.
standup_time = min(reset_time, 1.5)
for t in np.arange(0, reset_time,
self.time_step * self._action_repeat):
blend_ratio = min(t / standup_time, 1)
action = blend_ratio * default_motor_angles + (
1 - blend_ratio) * current_motor_angles
self.Step(action, robot_config.MotorControlMode.POSITION)
time.sleep(self.time_step * self._action_repeat)
if self._enable_action_filter:
self._ResetActionFilter()
self._velocity_estimator.reset()
self._state_action_counter = 0
self._step_counter = 0
self._last_reset_time = time.time()
def Terminate(self):
self._is_alive = False
def _StepInternal(self, action, motor_control_mode=None):
self.ApplyAction(action, motor_control_mode)
self.ReceiveObservation()
self._state_action_counter += 1
class A1(minitaur.Minitaur):
"""A simulation for the Laikago robot."""
# At high replanning frequency, inaccurate values of BODY_MASS/INERTIA
# doesn't seem to matter much. However, these values should be better tuned
# when the replan frequency is low (e.g. using a less beefy CPU).
MPC_BODY_MASS = 108 / 9.8
MPC_BODY_INERTIA = np.array(
(0.017, 0, 0, 0, 0.057, 0, 0, 0, 0.064)) * 0.1#* 2
MPC_BODY_HEIGHT = 0.24
MPC_VELOCITY_MULTIPLIER = 0.5
ACTION_CONFIG = [
locomotion_gym_config.ScalarField(name="FR_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="FR_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="FR_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
locomotion_gym_config.ScalarField(name="FL_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="FL_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="FL_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
locomotion_gym_config.ScalarField(name="RR_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="RR_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="RR_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
locomotion_gym_config.ScalarField(name="RL_hip_motor",
upper_bound=0.802851455917,
lower_bound=-0.802851455917),
locomotion_gym_config.ScalarField(name="RL_upper_joint",
upper_bound=4.18879020479,
lower_bound=-1.0471975512),
locomotion_gym_config.ScalarField(name="RL_lower_joint",
upper_bound=-0.916297857297,
lower_bound=-2.69653369433),
]
def __init__(
self,
pybullet_client,
urdf_filename=URDF_FILENAME,
enable_clip_motor_commands=False,
time_step=0.001,
action_repeat=10,
sensors=None,
control_latency=0.002,
on_rack=False,
enable_action_interpolation=True,
enable_action_filter=False,
motor_control_mode=None,
reset_time=1,
allow_knee_contact=False,
):
self._urdf_filename = urdf_filename
self._allow_knee_contact = allow_knee_contact
self._enable_clip_motor_commands = enable_clip_motor_commands
motor_kp = [
ABDUCTION_P_GAIN, HIP_P_GAIN, KNEE_P_GAIN, ABDUCTION_P_GAIN,
HIP_P_GAIN, KNEE_P_GAIN, ABDUCTION_P_GAIN, HIP_P_GAIN, KNEE_P_GAIN,
ABDUCTION_P_GAIN, HIP_P_GAIN, KNEE_P_GAIN
]
motor_kd = [
ABDUCTION_D_GAIN, HIP_D_GAIN, KNEE_D_GAIN, ABDUCTION_D_GAIN,
HIP_D_GAIN, KNEE_D_GAIN, ABDUCTION_D_GAIN, HIP_D_GAIN, KNEE_D_GAIN,
ABDUCTION_D_GAIN, HIP_D_GAIN, KNEE_D_GAIN
]
super(A1, self).__init__(
pybullet_client=pybullet_client,
time_step=time_step,
action_repeat=action_repeat,
num_motors=NUM_MOTORS,
dofs_per_leg=DOFS_PER_LEG,
motor_direction=JOINT_DIRECTIONS,
motor_offset=JOINT_OFFSETS,
motor_overheat_protection=False,
motor_control_mode=motor_control_mode,
motor_model_class=laikago_motor.LaikagoMotorModel,
sensors=sensors,
motor_kp=motor_kp,
motor_kd=motor_kd,
control_latency=control_latency,
on_rack=on_rack,
enable_action_interpolation=enable_action_interpolation,
enable_action_filter=enable_action_filter,
reset_time=reset_time)
def _LoadRobotURDF(self):
a1_urdf_path = self.GetURDFFile()
if self._self_collision_enabled:
self.quadruped = self._pybullet_client.loadURDF(
a1_urdf_path,
self._GetDefaultInitPosition(),
self._GetDefaultInitOrientation(),
flags=self._pybullet_client.URDF_USE_SELF_COLLISION)
else:
self.quadruped = self._pybullet_client.loadURDF(
a1_urdf_path, self._GetDefaultInitPosition(),
self._GetDefaultInitOrientation())
def _SettleDownForReset(self, default_motor_angles, reset_time):
self.ReceiveObservation()
if reset_time <= 0:
return
for _ in range(500):
self._StepInternal(
INIT_MOTOR_ANGLES,
motor_control_mode=robot_config.MotorControlMode.POSITION)
if default_motor_angles is not None:
num_steps_to_reset = int(reset_time / self.time_step)
for _ in range(num_steps_to_reset):
self._StepInternal(
default_motor_angles,
motor_control_mode=robot_config.MotorControlMode.POSITION)
def GetHipPositionsInBaseFrame(self):
return _DEFAULT_HIP_POSITIONS
def GetFootContacts(self):
all_contacts = self._pybullet_client.getContactPoints(bodyA=self.quadruped)
contacts = [False, False, False, False]
for contact in all_contacts:
# Ignore self contacts
if contact[_BODY_B_FIELD_NUMBER] == self.quadruped:
continue
try:
toe_link_index = self._foot_link_ids.index(
contact[_LINK_A_FIELD_NUMBER])
contacts[toe_link_index] = True
except ValueError:
continue
return contacts
def ResetPose(self, add_constraint):
del add_constraint
for name in self._joint_name_to_id:
joint_id = self._joint_name_to_id[name]
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=(joint_id),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
for name, i in zip(MOTOR_NAMES, range(len(MOTOR_NAMES))):
if "hip_joint" in name:
angle = INIT_MOTOR_ANGLES[i] + HIP_JOINT_OFFSET
elif "upper_joint" in name:
angle = INIT_MOTOR_ANGLES[i] + UPPER_LEG_JOINT_OFFSET
elif "lower_joint" in name:
angle = INIT_MOTOR_ANGLES[i] + KNEE_JOINT_OFFSET
else:
raise ValueError("The name %s is not recognized as a motor joint." %
name)
self._pybullet_client.resetJointState(self.quadruped,
self._joint_name_to_id[name],
angle,
targetVelocity=0)
def GetURDFFile(self):
return self._urdf_filename
def _BuildUrdfIds(self):
"""Build the link Ids from its name in the URDF file.
Raises:
ValueError: Unknown category of the joint name.
"""
num_joints = self.pybullet_client.getNumJoints(self.quadruped)
self._hip_link_ids = [-1]
self._leg_link_ids = []
self._motor_link_ids = []
self._lower_link_ids = []
self._foot_link_ids = []
self._imu_link_ids = []
for i in range(num_joints):
joint_info = self.pybullet_client.getJointInfo(self.quadruped, i)
joint_name = joint_info[1].decode("UTF-8")
joint_id = self._joint_name_to_id[joint_name]
if HIP_NAME_PATTERN.match(joint_name):
self._hip_link_ids.append(joint_id)
elif UPPER_NAME_PATTERN.match(joint_name):
self._motor_link_ids.append(joint_id)
# We either treat the lower leg or the toe as the foot link, depending on
# the urdf version used.
elif LOWER_NAME_PATTERN.match(joint_name):
self._lower_link_ids.append(joint_id)
elif TOE_NAME_PATTERN.match(joint_name):
#assert self._urdf_filename == URDF_WITH_TOES
self._foot_link_ids.append(joint_id)
elif IMU_NAME_PATTERN.match(joint_name):
self._imu_link_ids.append(joint_id)
else:
raise ValueError("Unknown category of joint %s" % joint_name)
self._leg_link_ids.extend(self._lower_link_ids)
self._leg_link_ids.extend(self._foot_link_ids)
#assert len(self._foot_link_ids) == NUM_LEGS
self._hip_link_ids.sort()
self._motor_link_ids.sort()
self._lower_link_ids.sort()
self._foot_link_ids.sort()
self._leg_link_ids.sort()
def _GetMotorNames(self):
return MOTOR_NAMES
def _GetDefaultInitPosition(self):
if self._on_rack:
return INIT_RACK_POSITION
else:
return INIT_POSITION
def _GetDefaultInitOrientation(self):
# The Laikago URDF assumes the initial pose of heading towards z axis,
# and belly towards y axis. The following transformation is to transform
# the Laikago initial orientation to our commonly used orientation: heading
# towards -x direction, and z axis is the up direction.
init_orientation = pyb.getQuaternionFromEuler([0., 0., 0.])
return init_orientation
def GetDefaultInitPosition(self):
"""Get default initial base position."""
return self._GetDefaultInitPosition()
def GetDefaultInitOrientation(self):
"""Get default initial base orientation."""
return self._GetDefaultInitOrientation()
def GetDefaultInitJointPose(self):
"""Get default initial joint pose."""
joint_pose = (INIT_MOTOR_ANGLES + JOINT_OFFSETS) * JOINT_DIRECTIONS
return joint_pose
def ApplyAction(self, motor_commands, motor_control_mode=None):
"""Clips and then apply the motor commands using the motor model.
Args:
motor_commands: np.array. Can be motor angles, torques, hybrid commands,
or motor pwms (for Minitaur only).N
motor_control_mode: A MotorControlMode enum.
"""
if self._enable_clip_motor_commands:
motor_commands = self._ClipMotorCommands(motor_commands)
super(A1, self).ApplyAction(motor_commands, motor_control_mode)
def _ClipMotorCommands(self, motor_commands):
"""Clips motor commands.
Args:
motor_commands: np.array. Can be motor angles, torques, hybrid commands,
or motor pwms (for Minitaur only).
Returns:
Clipped motor commands.
"""
# clamp the motor command by the joint limit, in case weired things happens
max_angle_change = MAX_MOTOR_ANGLE_CHANGE_PER_STEP
current_motor_angles = self.GetMotorAngles()
motor_commands = np.clip(motor_commands,
current_motor_angles - max_angle_change,
current_motor_angles + max_angle_change)
return motor_commands
@classmethod
def GetConstants(cls):
del cls
return laikago_constants