"""
CODE BASED ON EXAMPLE FROM:
@misc{coumans2017pybullet,
title={Pybullet, a python module for physics simulation in robotics, games and machine learning},
author={Coumans, Erwin and Bai, Yunfei},
url={www.pybullet.org},
year={2017},
}
Example: minitaur.py
https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py
"""
import collections
import copy
import math
import re
import numpy as np
from . import motor
from spotmicro.util import pybullet_data
print(pybullet_data.getDataPath())
from spotmicro.Kinematics.SpotKinematics import SpotModel
import spotmicro.Kinematics.LieAlgebra as LA
INIT_POSITION = [0, 0, 0.25]
INIT_RACK_POSITION = [0, 0, 1]
# NOTE: URDF IS FACING THE WRONG WAY
# TEMP FIX
INIT_ORIENTATION = [0, 0, 0, 1]
OVERHEAT_SHUTDOWN_TORQUE = 2.45
OVERHEAT_SHUTDOWN_TIME = 1.0
# -math.pi / 5
INIT_LEG_POS = -0.658319
# math.pi / 3
INIT_FOOT_POS = 1.0472
OLD_LEG_POSITION = ["front_left", "front_right", "rear_left", "rear_right"]
OLD_MOTOR_NAMES = [
"motor_front_left_shoulder", "motor_front_left_leg",
"foot_motor_front_left", "motor_front_right_shoulder",
"motor_front_right_leg", "foot_motor_front_right",
"motor_rear_left_shoulder", "motor_rear_left_leg", "foot_motor_rear_left",
"motor_rear_right_shoulder", "motor_rear_right_leg",
"foot_motor_rear_right"
]
OLD_MOTOR_LIMITS_BY_NAME = {}
for name in OLD_MOTOR_NAMES:
if "shoulder" in name:
OLD_MOTOR_LIMITS_BY_NAME[name] = [-1.04, 1.04]
elif "leg" in name:
OLD_MOTOR_LIMITS_BY_NAME[name] = [-2.59, 1.571]
elif "foot" in name:
OLD_MOTOR_LIMITS_BY_NAME[name] = [-1.571, 2.9]
OLD_FOOT_NAMES = [
"front_left_toe", "front_right_toe", "rear_left_toe", "rear_right_toe"
]
LEG_POSITION = ["front_left", "front_right", "back_left", "back_right"]
MOTOR_NAMES = [
"motor_front_left_hip", "motor_front_left_upper_leg",
"motor_front_left_lower_leg", "motor_front_right_hip",
"motor_front_right_upper_leg", "motor_front_right_lower_leg",
"motor_back_left_hip", "motor_back_left_upper_leg",
"motor_back_left_lower_leg", "motor_back_right_hip",
"motor_back_right_upper_leg", "motor_back_right_lower_leg"
]
MOTOR_LIMITS_BY_NAME = {}
for name in MOTOR_NAMES:
if "hip" in name:
MOTOR_LIMITS_BY_NAME[name] = [-1.04, 1.04]
elif "upper_leg" in name:
MOTOR_LIMITS_BY_NAME[name] = [-1.571, 2.59]
elif "lower_leg" in name:
MOTOR_LIMITS_BY_NAME[name] = [-2.9, 1.671]
FOOT_NAMES = [
"front_left_leg_foot", "front_right_leg_foot", "back_left_leg_foot",
"back_right_leg_foot"
]
_CHASSIS_NAME_PATTERN = re.compile(r"chassis\D*")
_MOTOR_NAME_PATTERN = re.compile(r"motor\D*")
_FOOT_NAME_PATTERN = re.compile(r"foot\D*")
SENSOR_NOISE_STDDEV = (0.0, 0.0, 0.0, 0.0, 0.0)
TWO_PI = 2 * math.pi
[docs]def MapToMinusPiToPi(angles):
"""Maps a list of angles to [-pi, pi].
Args:
angles: A list of angles in rad.
Returns:
A list of angle mapped to [-pi, pi].
"""
mapped_angles = copy.deepcopy(angles)
for i in range(len(angles)):
mapped_angles[i] = math.fmod(angles[i], TWO_PI)
if mapped_angles[i] >= math.pi:
mapped_angles[i] -= TWO_PI
elif mapped_angles[i] < -math.pi:
mapped_angles[i] += TWO_PI
return mapped_angles
[docs]class Spot(object):
"""The spot class that simulates a quadruped robot.
"""
INIT_POSES = {
'stand':
np.array([
0.15192765, 0.7552236, -1.5104472, -0.15192765, 0.7552236,
-1.5104472, 0.15192765, 0.7552236, -1.5104472, -0.15192765,
0.7552236, -1.5104472
]),
'liedown':
np.array([-0.4, -1.5, 6, 0.4, -1.5, 6, -0.4, -1.5, 6, 0.4, -1.5, 6]),
'zero':
np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
}
def __init__(self,
pybullet_client,
urdf_root=pybullet_data.getDataPath(),
time_step=0.01,
action_repeat=1,
self_collision_enabled=False,
motor_velocity_limit=9.7,
pd_control_enabled=False,
accurate_motor_model_enabled=False,
remove_default_joint_damping=False,
max_force=100.0,
motor_kp=1.0,
motor_kd=0.02,
pd_latency=0.0,
control_latency=0.0,
observation_noise_stdev=SENSOR_NOISE_STDDEV,
torque_control_enabled=False,
motor_overheat_protection=False,
on_rack=False,
kd_for_pd_controllers=0.3,
pose_id='stand',
np_random=np.random,
contacts=True):
"""Constructs a spot and reset it to the initial states.
Args:
pybullet_client: The instance of BulletClient to manage different
simulations.
urdf_root: The path to the urdf folder.
time_step: The time step of the simulation.
action_repeat: The number of ApplyAction() for each control step.
self_collision_enabled: Whether to enable self collision.
motor_velocity_limit: The upper limit of the motor velocity.
pd_control_enabled: Whether to use PD control for the motors.
accurate_motor_model_enabled: Whether to use the accurate DC motor model.
remove_default_joint_damping: Whether to remove the default joint damping.
motor_kp: proportional gain for the accurate motor model.
motor_kd: derivative gain for the accurate motor model.
pd_latency: The latency of the observations (in seconds) used to calculate
PD control. On the real hardware, it is the latency between the
microcontroller and the motor controller.
control_latency: The latency of the observations (in second) used to
calculate action. On the real hardware, it is the latency from the motor
controller, the microcontroller to the host (Nvidia TX2).
observation_noise_stdev: The standard deviation of a Gaussian noise model
for the sensor. It should be an array for separate sensors in the
following order [motor_angle, motor_velocity, motor_torque,
base_roll_pitch_yaw, base_angular_velocity]
torque_control_enabled: Whether to use the torque control, if set to
False, pose control will be used.
motor_overheat_protection: Whether to shutdown the motor that has exerted
large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
(OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in spot.py for more
details.
on_rack: Whether to place the spot on rack. This is only used to debug
the walking gait. In this mode, the spot's base is hanged midair so
that its walking gait is clearer to visualize.
"""
# SPOT MODEL
self.spot = SpotModel()
# Whether to include contact sensing
self.contacts = contacts
# Control Inputs
self.StepLength = 0.0
self.StepVelocity = 0.0
self.LateralFraction = 0.0
self.YawRate = 0.0
# Leg Phases
self.LegPhases = [0.0, 0.0, 0.0, 0.0]
# used to calculate minitaur acceleration
self.init_leg = INIT_LEG_POS
self.init_foot = INIT_FOOT_POS
self.prev_ang_twist = np.array([0, 0, 0])
self.prev_lin_twist = np.array([0, 0, 0])
self.prev_lin_acc = np.array([0, 0, 0])
self.num_motors = 12
self.num_legs = int(self.num_motors / 3)
self._pybullet_client = pybullet_client
self._action_repeat = action_repeat
self._urdf_root = urdf_root
self._self_collision_enabled = self_collision_enabled
self._motor_velocity_limit = motor_velocity_limit
self._pd_control_enabled = pd_control_enabled
self._motor_direction = np.ones(self.num_motors)
self._observed_motor_torques = np.zeros(self.num_motors)
self._applied_motor_torques = np.zeros(self.num_motors)
self._max_force = max_force
self._pd_latency = pd_latency
self._control_latency = control_latency
self._observation_noise_stdev = observation_noise_stdev
self._accurate_motor_model_enabled = accurate_motor_model_enabled
self._remove_default_joint_damping = remove_default_joint_damping
self._observation_history = collections.deque(maxlen=100)
self._control_observation = []
self._chassis_link_ids = [-1]
self._leg_link_ids = []
self._motor_link_ids = []
self._foot_link_ids = []
self._torque_control_enabled = torque_control_enabled
self._motor_overheat_protection = motor_overheat_protection
self._on_rack = on_rack
self._pose_id = pose_id
self.np_random = np_random
if self._accurate_motor_model_enabled:
self._kp = motor_kp
self._kd = motor_kd
self._motor_model = motor.MotorModel(
torque_control_enabled=self._torque_control_enabled,
kp=self._kp,
kd=self._kd)
elif self._pd_control_enabled:
self._kp = 8
self._kd = kd_for_pd_controllers
else:
self._kp = 1
self._kd = 1
self.time_step = time_step
self._step_counter = 0
# reset_time=-1.0 means skipping the reset motion.
# See Reset for more details.
self.Reset(reset_time=-1)
self.init_on_rack_position = INIT_RACK_POSITION
self.init_position = INIT_POSITION
self.initial_pose = self.INIT_POSES[pose_id]
def _RecordMassInfoFromURDF(self):
self._base_mass_urdf = []
for chassis_id in self._chassis_link_ids:
self._base_mass_urdf.append(
self._pybullet_client.getDynamicsInfo(self.quadruped,
chassis_id)[0])
self._leg_masses_urdf = []
for leg_id in self._leg_link_ids:
self._leg_masses_urdf.append(
self._pybullet_client.getDynamicsInfo(self.quadruped,
leg_id)[0])
for motor_id in self._motor_link_ids:
self._leg_masses_urdf.append(
self._pybullet_client.getDynamicsInfo(self.quadruped,
motor_id)[0])
[docs] def GetBaseMassFromURDF(self):
"""Get the mass of the base from the URDF file."""
return self._base_mass_urdf
[docs] def SetBaseMass(self, base_mass):
for i in range(len(self._chassis_link_ids)):
self._pybullet_client.changeDynamics(self.quadruped,
self._chassis_link_ids[i],
mass=base_mass[i])
def _RecordInertiaInfoFromURDF(self):
"""Record the inertia of each body from URDF file."""
self._link_urdf = []
num_bodies = self._pybullet_client.getNumJoints(self.quadruped)
for body_id in range(-1, num_bodies): # -1 is for the base link.
inertia = self._pybullet_client.getDynamicsInfo(
self.quadruped, body_id)[2]
self._link_urdf.append(inertia)
# We need to use id+1 to index self._link_urdf because it has the base
# (index = -1) at the first element.
self._base_inertia_urdf = [
self._link_urdf[chassis_id + 1]
for chassis_id in self._chassis_link_ids
]
self._leg_inertia_urdf = [
self._link_urdf[leg_id + 1] for leg_id in self._leg_link_ids
]
self._leg_inertia_urdf.extend([
self._link_urdf[motor_id + 1] for motor_id in self._motor_link_ids
])
def _BuildJointNameToIdDict(self):
num_joints = self._pybullet_client.getNumJoints(self.quadruped)
self._joint_name_to_id = {}
for i in range(num_joints):
joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
self._joint_name_to_id[joint_info[1].decode(
"UTF-8")] = joint_info[0]
def _BuildMotorIdList(self):
self._motor_id_list = [
self._joint_name_to_id[motor_name] for motor_name in MOTOR_NAMES
]
def _BuildFootIdList(self):
self._foot_id_list = [
self._joint_name_to_id[foot_name] for foot_name in FOOT_NAMES
]
print(self._foot_id_list)
def _BuildUrdfIds(self):
"""Build the link Ids from its name in the URDF file."""
c = []
m = []
f = []
lg = []
num_joints = self._pybullet_client.getNumJoints(self.quadruped)
self._chassis_link_ids = [-1]
# the self._leg_link_ids include both the upper and lower links of the leg.
self._leg_link_ids = []
self._motor_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):
c.append(joint_name)
self._chassis_link_ids.append(joint_id)
elif _MOTOR_NAME_PATTERN.match(joint_name):
m.append(joint_name)
self._motor_link_ids.append(joint_id)
elif _FOOT_NAME_PATTERN.match(joint_name):
f.append(joint_name)
self._foot_link_ids.append(joint_id)
else:
lg.append(joint_name)
self._leg_link_ids.append(joint_id)
self._leg_link_ids.extend(self._foot_link_ids)
self._chassis_link_ids.sort()
self._motor_link_ids.sort()
self._foot_link_ids.sort()
self._leg_link_ids.sort()
[docs] def Reset(self,
reload_urdf=True,
default_motor_angles=None,
reset_time=3.0):
"""Reset the spot to its initial states.
Args:
reload_urdf: Whether to reload the urdf file. If not, Reset() just place
the spot back to its starting position.
default_motor_angles: The default motor angles. If it is None, spot
will hold a default pose for 100 steps. In
torque control mode, the phase of holding the default pose is skipped.
reset_time: The duration (in seconds) to hold the default motor angles. If
reset_time <= 0 or in torque control mode, the phase of holding the
default pose is skipped.
"""
if self._on_rack:
init_position = INIT_RACK_POSITION
else:
init_position = INIT_POSITION
if reload_urdf:
if self._self_collision_enabled:
self.quadruped = self._pybullet_client.loadURDF(
pybullet_data.getDataPath() + "/assets/urdf/spot.urdf",
init_position,
useFixedBase=self._on_rack,
flags=self._pybullet_client.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
else:
self.quadruped = self._pybullet_client.loadURDF(
pybullet_data.getDataPath() + "/assets/urdf/spot.urdf",
init_position,
INIT_ORIENTATION,
useFixedBase=self._on_rack)
self._BuildJointNameToIdDict()
self._BuildUrdfIds()
if self._remove_default_joint_damping:
self._RemoveDefaultJointDamping()
self._BuildMotorIdList()
self._BuildFootIdList()
self._RecordMassInfoFromURDF()
self._RecordInertiaInfoFromURDF()
self.ResetPose(add_constraint=True)
else:
self._pybullet_client.resetBasePositionAndOrientation(
self.quadruped, init_position, INIT_ORIENTATION)
self._pybullet_client.resetBaseVelocity(self.quadruped, [0, 0, 0],
[0, 0, 0])
# self._pybullet_client.changeDynamics(self.quadruped, -1, lateralFriction=0.8)
self.ResetPose(add_constraint=False)
self._overheat_counter = np.zeros(self.num_motors)
self._motor_enabled_list = [True] * self.num_motors
self._step_counter = 0
# Perform reset motion within reset_duration if in position control mode.
# Nothing is performed if in torque control mode for now.
self._observation_history.clear()
if reset_time > 0.0 and default_motor_angles is not None:
self.RealisticObservation()
for _ in range(100):
self.ApplyAction(self.initial_pose)
self._pybullet_client.stepSimulation()
self.RealisticObservation()
num_steps_to_reset = int(reset_time / self.time_step)
for _ in range(num_steps_to_reset):
self.ApplyAction(default_motor_angles)
self._pybullet_client.stepSimulation()
self.RealisticObservation()
self.RealisticObservation()
# Set Foot Friction
self.SetFootFriction()
def _RemoveDefaultJointDamping(self):
num_joints = self._pybullet_client.getNumJoints(self.quadruped)
for i in range(num_joints):
joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
self._pybullet_client.changeDynamics(joint_info[0],
-1,
linearDamping=0,
angularDamping=0)
def _SetMotorTorqueById(self, motor_id, torque):
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=motor_id,
controlMode=self._pybullet_client.TORQUE_CONTROL,
force=torque)
def _SetDesiredMotorAngleById(self, motor_id, desired_angle):
if self._pd_control_enabled or self._accurate_motor_model_enabled:
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=motor_id,
controlMode=self._pybullet_client.POSITION_CONTROL,
targetPosition=desired_angle,
positionGain=self._kp,
velocityGain=self._kd,
force=self._max_force)
# Pybullet has a 'perfect' joint controller with its default p,d
else:
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=motor_id,
controlMode=self._pybullet_client.POSITION_CONTROL,
targetPosition=desired_angle)
def _SetDesiredMotorAngleByName(self, motor_name, desired_angle):
self._SetDesiredMotorAngleById(self._joint_name_to_id[motor_name],
desired_angle)
[docs] def ResetPose(self, add_constraint):
"""Reset the pose of the spot.
Args:
add_constraint: Whether to add a constraint at the joints of two feet.
"""
for i in range(self.num_legs):
self._ResetPoseForLeg(i, add_constraint)
def _ResetPoseForLeg(self, leg_id, add_constraint):
"""Reset the initial pose for the leg.
Args:
leg_id: It should be 0, 1, 2, or 3, which represents the leg at
front_left, back_left, front_right and back_right.
add_constraint: Whether to add a constraint at the joints of two feet.
"""
knee_friction_force = 0
pi = math.pi
leg_position = LEG_POSITION[leg_id]
self._pybullet_client.resetJointState(
self.quadruped,
self._joint_name_to_id["motor_" + leg_position + "_hip"],
self.INIT_POSES[self._pose_id][3 * leg_id],
targetVelocity=0)
self._pybullet_client.resetJointState(
self.quadruped,
self._joint_name_to_id["motor_" + leg_position + "_upper_leg"],
self.INIT_POSES[self._pose_id][3 * leg_id + 1],
targetVelocity=0)
self._pybullet_client.resetJointState(
self.quadruped,
self._joint_name_to_id["motor_" + leg_position + "_lower_leg"],
self.INIT_POSES[self._pose_id][3 * leg_id + 2],
targetVelocity=0)
if self._accurate_motor_model_enabled or self._pd_control_enabled:
# Disable the default motor in pybullet.
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=(self._joint_name_to_id["motor_" + leg_position +
"_hip"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
targetVelocity=0,
force=knee_friction_force)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=(self._joint_name_to_id["motor_" + leg_position +
"_upper_leg"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
targetVelocity=0,
force=knee_friction_force)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=(self._joint_name_to_id["motor_" + leg_position +
"_lower_leg"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
targetVelocity=0,
force=knee_friction_force)
[docs] def GetBasePosition(self):
"""Get the position of spot's base.
Returns:
The position of spot's base.
"""
position, _ = (self._pybullet_client.getBasePositionAndOrientation(
self.quadruped))
return position
[docs] def GetBaseOrientation(self):
"""Get the orientation of spot's base, represented as quaternion.
Returns:
The orientation of spot's base.
"""
_, orientation = (self._pybullet_client.getBasePositionAndOrientation(
self.quadruped))
return orientation
[docs] def GetBaseRollPitchYaw(self):
"""Get the rate of orientation change of the spot's base in euler angle.
Returns:
rate of (roll, pitch, yaw) change of the spot's base.
"""
vel = self._pybullet_client.getBaseVelocity(self.quadruped)
return np.asarray([vel[1][0], vel[1][1], vel[1][2]])
[docs] def GetBaseRollPitchYawRate(self):
"""Get the rate of orientation change of the spot's base in euler angle.
This function mimicks the noisy sensor reading and adds latency.
Returns:
rate of (roll, pitch, yaw) change of the spot's base polluted by noise
and latency.
"""
return self._AddSensorNoise(
np.array(self._control_observation[3 * self.num_motors +
4:3 * self.num_motors + 7]),
self._observation_noise_stdev[4])
[docs] def GetBaseTwist(self):
"""Get the Twist of minitaur's base.
Returns:
The Twist of the minitaur's base.
"""
return self._pybullet_client.getBaseVelocity(self.quadruped)
[docs] def GetActionDimension(self):
"""Get the length of the action list.
Returns:
The length of the action list.
"""
return self.num_motors
[docs] def GetObservationUpperBound(self):
"""Get the upper bound of the observation.
Returns:
The upper bound of an observation. See GetObservation() for the details
of each element of an observation.
NOTE: Changed just like GetObservation()
"""
upper_bound = np.array([0.0] * self.GetObservationDimension())
# roll, pitch
upper_bound[0:2] = 2.0 * np.pi
# acc, rate in x,y,z
upper_bound[2:8] = np.inf
# Leg Phases
upper_bound[8:12] = 2.0
# Contacts
if self.contacts:
upper_bound[12:] = 1.0
return upper_bound
[docs] def GetObservationLowerBound(self):
"""Get the lower bound of the observation."""
return -self.GetObservationUpperBound()
[docs] def GetObservationDimension(self):
"""Get the length of the observation list.
Returns:
The length of the observation list.
"""
return len(self.GetObservation())
[docs] def GetObservation(self):
"""Get the observations of minitaur.
It includes the angles, velocities, torques and the orientation of the base.
Returns:
The observation list. observation[0:8] are motor angles. observation[8:16]
are motor velocities, observation[16:24] are motor torques.
observation[24:28] is the orientation of the base, in quaternion form.
NOTE: DIVERGES FROM STOCK MINITAUR ENV. WILL LEAVE ORIGINAL COMMENTED
For my purpose, the observation space includes Roll and Pitch, as well as
acceleration and gyroscopic rate along the x,y,z axes. All of this
information can be collected from an onboard IMU. The reward function
will contain a hidden velocity reward (fwd, bwd) which cannot be measured
and so is not included. For spinning, the gyroscopic z rate will be used
as the (explicit) velocity reward.
This version operates without motor torques, angles and velocities. Erwin
Coumans' paper suggests a sparse observation space leads to higher reward
# NOTE: use True version for perfect data, or other for realistic data
"""
observation = []
# GETTING TWIST IN BODY FRAME
pos = self.GetBasePosition()
orn = self.GetBaseOrientation()
roll, pitch, yaw = self._pybullet_client.getEulerFromQuaternion(
[orn[0], orn[1], orn[2], orn[3]])
# rpy = LA.RPY(roll, pitch, yaw)
# R, _ = LA.TransToRp(rpy)
# T_wb = LA.RpToTrans(R, np.array([pos[0], pos[1], pos[2]]))
# T_bw = LA.TransInv(T_wb)
# Adj_Tbw = LA.Adjoint(T_bw)
# Get Linear and Angular Twist in WORLD FRAME
lin_twist, ang_twist = self.GetBaseTwist()
lin_twist = np.array([lin_twist[0], lin_twist[1], lin_twist[2]])
ang_twist = np.array([ang_twist[0], ang_twist[1], ang_twist[2]])
# Vw = np.concatenate((ang_twist, lin_twist))
# Vb = np.dot(Adj_Tbw, Vw)
# roll, pitch, _ = self._pybullet_client.getEulerFromQuaternion(
# [orn[0], orn[1], orn[2], orn[3]])
# # Get linear accelerations
# lin_twist = -Vb[3:]
# ang_twist = Vb[:3]
lin_acc = lin_twist - self.prev_lin_twist
if lin_acc.all() == 0.0:
lin_acc = self.prev_lin_acc
self.prev_lin_acc = lin_acc
# print("LIN TWIST: ", lin_twist)
self.prev_lin_twist = lin_twist
self.prev_ang_twist = ang_twist
# Get Contacts
CONTACT = list(self._pybullet_client.getContactPoints(self.quadruped))
FLC = 0
FRC = 0
BLC = 0
BRC = 0
if len(CONTACT) > 0:
for i in range(len(CONTACT)):
Contact_Link_Index = CONTACT[i][3]
if Contact_Link_Index == self._foot_id_list[0]:
FLC = 1
# print("FL CONTACT")
if Contact_Link_Index == self._foot_id_list[1]:
FRC = 1
# print("FR CONTACT")
if Contact_Link_Index == self._foot_id_list[2]:
BLC = 1
# print("BL CONTACT")
if Contact_Link_Index == self._foot_id_list[3]:
BRC = 1
# print("BR CONTACT")
# order: roll, pitch, gyro(x,y,z), acc(x, y, z)
observation.append(roll)
observation.append(pitch)
observation.extend(list(ang_twist))
observation.extend(list(lin_acc))
# Control Input
# observation.append(self.StepLength)
# observation.append(self.StepVelocity)
# observation.append(self.LateralFraction)
# observation.append(self.YawRate)
observation.extend(self.LegPhases)
if self.contacts:
observation.append(FLC)
observation.append(FRC)
observation.append(BLC)
observation.append(BRC)
# print("CONTACTS: {} {} {} {}".format(FLC, FRC, BLC, BRC))
return observation
[docs] def GetLegPhases(self, TrajectoryGenerator):
""" Leg phases according to TG from 0->2
0->1: Stance
1->2 Swing
"""
self.LegPhases = TrajectoryGenerator.Phases
[docs] def GetExternalObservations(self, TrajectoryGenerator, controller):
""" Augment State Space
"""
self.GetControlInput(controller)
self.GetLegPhases(TrajectoryGenerator)
[docs] def ConvertFromLegModel(self, action):
# TODO
joint_angles = action
return joint_angles
[docs] def ApplyMotorLimits(self, joint_angles):
eps = 0.001
for i in range(len(joint_angles)):
LIM = MOTOR_LIMITS_BY_NAME[MOTOR_NAMES[i]]
joint_angles[i] = np.clip(joint_angles[i], LIM[0] + eps,
LIM[1] - eps)
return joint_angles
[docs] def ApplyAction(self, motor_commands):
"""Set the desired motor angles to the motors of the minitaur.
The desired motor angles are clipped based on the maximum allowed velocity.
If the pd_control_enabled is True, a torque is calculated according to
the difference between current and desired joint angle, as well as the joint
velocity. This torque is exerted to the motor. For more information about
PD control, please refer to: https://en.wikipedia.org/wiki/PID_controller.
Args:
motor_commands: The eight desired motor angles.
"""
# FIRST, APPLY MOTOR LIMITS:
motor_commands = self.ApplyMotorLimits(motor_commands)
if self._motor_velocity_limit < np.inf:
current_motor_angle = self.GetMotorAngles()
motor_commands_max = (current_motor_angle +
self.time_step * self._motor_velocity_limit)
motor_commands_min = (current_motor_angle -
self.time_step * self._motor_velocity_limit)
motor_commands = np.clip(motor_commands, motor_commands_min,
motor_commands_max)
if self._accurate_motor_model_enabled or self._pd_control_enabled:
q = self.GetMotorAngles()
qdot = self.GetMotorVelocities()
if self._accurate_motor_model_enabled:
actual_torque, observed_torque = self._motor_model.convert_to_torque(
motor_commands, q, qdot)
if self._motor_overheat_protection:
for i in range(self.num_motors):
if abs(actual_torque[i]) > OVERHEAT_SHUTDOWN_TORQUE:
self._overheat_counter[i] += 1
else:
self._overheat_counter[i] = 0
if (self._overheat_counter[i] >
OVERHEAT_SHUTDOWN_TIME / self.time_step):
self._motor_enabled_list[i] = False
# The torque is already in the observation space because we use
# GetMotorAngles and GetMotorVelocities.
self._observed_motor_torques = observed_torque
# Transform into the motor space when applying the torque.
self._applied_motor_torque = np.multiply(
actual_torque, self._motor_direction)
for motor_id, motor_torque, motor_enabled in zip(
self._motor_id_list, self._applied_motor_torque,
self._motor_enabled_list):
if motor_enabled:
self._SetMotorTorqueById(motor_id, motor_torque)
else:
self._SetMotorTorqueById(motor_id, 0)
else:
torque_commands = -self._kp * (
q - motor_commands) - self._kd * qdot
# The torque is already in the observation space because we use
# GetMotorAngles and GetMotorVelocities.
self._observed_motor_torques = torque_commands
# Transform into the motor space when applying the torque.
self._applied_motor_torques = np.multiply(
self._observed_motor_torques, self._motor_direction)
for motor_id, motor_torque in zip(self._motor_id_list,
self._applied_motor_torques):
self._SetMotorTorqueById(motor_id, motor_torque)
else:
motor_commands_with_direction = np.multiply(
motor_commands, self._motor_direction)
for motor_id, motor_command_with_direction in zip(
self._motor_id_list, motor_commands_with_direction):
self._SetDesiredMotorAngleById(motor_id,
motor_command_with_direction)
[docs] def Step(self, action):
for _ in range(self._action_repeat):
self.ApplyAction(action)
self._pybullet_client.stepSimulation()
self.RealisticObservation()
self._step_counter += 1
[docs] def GetTimeSinceReset(self):
return self._step_counter * self.time_step
[docs] def GetMotorAngles(self):
"""Gets the eight motor angles at the current moment, mapped to [-pi, pi].
Returns:
Motor angles, mapped to [-pi, pi].
"""
motor_angles = [
self._pybullet_client.getJointState(self.quadruped, motor_id)[0]
for motor_id in self._motor_id_list
]
motor_angles = np.multiply(motor_angles, self._motor_direction)
return MapToMinusPiToPi(motor_angles)
[docs] def GetMotorVelocities(self):
"""Get the velocity of all eight motors.
Returns:
Velocities of all eight motors.
"""
motor_velocities = [
self._pybullet_client.getJointState(self.quadruped, motor_id)[1]
for motor_id in self._motor_id_list
]
motor_velocities = np.multiply(motor_velocities, self._motor_direction)
return motor_velocities
[docs] def GetMotorTorques(self):
"""Get the amount of torque the motors are exerting.
Returns:
Motor torques of all eight motors.
"""
if self._accurate_motor_model_enabled or self._pd_control_enabled:
return self._observed_motor_torques
else:
motor_torques = [
self._pybullet_client.getJointState(self.quadruped,
motor_id)[3]
for motor_id in self._motor_id_list
]
motor_torques = np.multiply(motor_torques, self._motor_direction)
return motor_torques
[docs] def GetBaseMassesFromURDF(self):
"""Get the mass of the base from the URDF file."""
return self._base_mass_urdf
[docs] def GetBaseInertiasFromURDF(self):
"""Get the inertia of the base from the URDF file."""
return self._base_inertia_urdf
[docs] def GetLegMassesFromURDF(self):
"""Get the mass of the legs from the URDF file."""
return self._leg_masses_urdf
[docs] def GetLegInertiasFromURDF(self):
"""Get the inertia of the legs from the URDF file."""
return self._leg_inertia_urdf
[docs] def SetBaseMasses(self, base_mass):
"""Set the mass of spot's base.
Args:
base_mass: A list of masses of each body link in CHASIS_LINK_IDS. The
length of this list should be the same as the length of CHASIS_LINK_IDS.
Raises:
ValueError: It is raised when the length of base_mass is not the same as
the length of self._chassis_link_ids.
"""
if len(base_mass) != len(self._chassis_link_ids):
raise ValueError(
"The length of base_mass {} and self._chassis_link_ids {} are not "
"the same.".format(len(base_mass),
len(self._chassis_link_ids)))
for chassis_id, chassis_mass in zip(self._chassis_link_ids, base_mass):
self._pybullet_client.changeDynamics(self.quadruped,
chassis_id,
mass=chassis_mass)
[docs] def SetLegMasses(self, leg_masses):
"""Set the mass of the legs.
Args:
leg_masses: The leg and motor masses for all the leg links and motors.
Raises:
ValueError: It is raised when the length of masses is not equal to number
of links + motors.
"""
if len(leg_masses) != len(self._leg_link_ids) + len(
self._motor_link_ids):
raise ValueError("The number of values passed to SetLegMasses are "
"different than number of leg links and motors.")
for leg_id, leg_mass in zip(self._leg_link_ids, leg_masses):
self._pybullet_client.changeDynamics(self.quadruped,
leg_id,
mass=leg_mass)
motor_masses = leg_masses[len(self._leg_link_ids):]
for link_id, motor_mass in zip(self._motor_link_ids, motor_masses):
self._pybullet_client.changeDynamics(self.quadruped,
link_id,
mass=motor_mass)
[docs] def SetBaseInertias(self, base_inertias):
"""Set the inertias of spot's base.
Args:
base_inertias: A list of inertias of each body link in CHASIS_LINK_IDS.
The length of this list should be the same as the length of
CHASIS_LINK_IDS.
Raises:
ValueError: It is raised when the length of base_inertias is not the same
as the length of self._chassis_link_ids and base_inertias contains
negative values.
"""
if len(base_inertias) != len(self._chassis_link_ids):
raise ValueError(
"The length of base_inertias {} and self._chassis_link_ids {} are "
"not the same.".format(len(base_inertias),
len(self._chassis_link_ids)))
for chassis_id, chassis_inertia in zip(self._chassis_link_ids,
base_inertias):
for inertia_value in chassis_inertia:
if (np.asarray(inertia_value) < 0).any():
raise ValueError(
"Values in inertia matrix should be non-negative.")
self._pybullet_client.changeDynamics(
self.quadruped,
chassis_id,
localInertiaDiagonal=chassis_inertia)
[docs] def SetLegInertias(self, leg_inertias):
"""Set the inertias of the legs.
Args:
leg_inertias: The leg and motor inertias for all the leg links and motors.
Raises:
ValueError: It is raised when the length of inertias is not equal to
the number of links + motors or leg_inertias contains negative values.
"""
if len(leg_inertias) != len(self._leg_link_ids) + len(
self._motor_link_ids):
raise ValueError("The number of values passed to SetLegMasses are "
"different than number of leg links and motors.")
for leg_id, leg_inertia in zip(self._leg_link_ids, leg_inertias):
for inertia_value in leg_inertias:
if (np.asarray(inertia_value) < 0).any():
raise ValueError(
"Values in inertia matrix should be non-negative.")
self._pybullet_client.changeDynamics(
self.quadruped, leg_id, localInertiaDiagonal=leg_inertia)
motor_inertias = leg_inertias[len(self._leg_link_ids):]
for link_id, motor_inertia in zip(self._motor_link_ids,
motor_inertias):
for inertia_value in motor_inertias:
if (np.asarray(inertia_value) < 0).any():
raise ValueError(
"Values in inertia matrix should be non-negative.")
self._pybullet_client.changeDynamics(
self.quadruped, link_id, localInertiaDiagonal=motor_inertia)
# TODO(b/73748980): Add more API's to set other contact parameters.
[docs] def SetJointFriction(self, joint_frictions):
for knee_joint_id, friction in zip(self._foot_link_ids,
joint_frictions):
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=knee_joint_id,
controlMode=self._pybullet_client.VELOCITY_CONTROL,
targetVelocity=0,
force=friction)
[docs] def GetNumKneeJoints(self):
return len(self._foot_link_ids)
[docs] def SetBatteryVoltage(self, voltage):
if self._accurate_motor_model_enabled:
self._motor_model.set_voltage(voltage)
[docs] def SetMotorViscousDamping(self, viscous_damping):
if self._accurate_motor_model_enabled:
self._motor_model.set_viscous_damping(viscous_damping)
[docs] def RealisticObservation(self):
"""Receive the observation from sensors.
This function is called once per step. The observations are only updated
when this function is called.
"""
self._observation_history.appendleft(self.GetObservation())
self._control_observation = self._GetDelayedObservation(
self._control_latency)
self._control_observation = self._AddSensorNoise(
self._control_observation, self._observation_noise_stdev)
return self._control_observation
def _GetDelayedObservation(self, latency):
"""Get observation that is delayed by the amount specified in latency.
Args:
latency: The latency (in seconds) of the delayed observation.
Returns:
observation: The observation which was actually latency seconds ago.
"""
if latency <= 0 or len(self._observation_history) == 1:
observation = self._observation_history[0]
else:
n_steps_ago = int(latency / self.time_step)
if n_steps_ago + 1 >= len(self._observation_history):
return self._observation_history[-1]
remaining_latency = latency - n_steps_ago * self.time_step
blend_alpha = remaining_latency / self.time_step
observation = (
(1.0 - blend_alpha) *
np.array(self._observation_history[n_steps_ago]) +
blend_alpha *
np.array(self._observation_history[n_steps_ago + 1]))
return observation
def _GetPDObservation(self):
pd_delayed_observation = self._GetDelayedObservation(self._pd_latency)
q = pd_delayed_observation[0:self.num_motors]
qdot = pd_delayed_observation[self.num_motors:2 * self.num_motors]
return (np.array(q), np.array(qdot))
def _AddSensorNoise(self, observation, noise_stdev):
# if self._observation_noise_stdev > 0:
# observation += (self.np_random.normal(scale=noise_stdev,
# size=observation.shape) *
# self.GetObservationUpperBound())
return observation
[docs] def SetControlLatency(self, latency):
"""Set the latency of the control loop.
It measures the duration between sending an action from Nvidia TX2 and
receiving the observation from microcontroller.
Args:
latency: The latency (in seconds) of the control loop.
"""
self._control_latency = latency
[docs] def GetControlLatency(self):
"""Get the control latency.
Returns:
The latency (in seconds) between when the motor command is sent and when
the sensor measurements are reported back to the controller.
"""
return self._control_latency
[docs] def SetMotorGains(self, kp, kd):
"""Set the gains of all motors.
These gains are PD gains for motor positional control. kp is the
proportional gain and kd is the derivative gain.
Args:
kp: proportional gain of the motors.
kd: derivative gain of the motors.
"""
self._kp = kp
self._kd = kd
if self._accurate_motor_model_enabled:
self._motor_model.set_motor_gains(kp, kd)
[docs] def GetMotorGains(self):
"""Get the gains of the motor.
Returns:
The proportional gain.
The derivative gain.
"""
return self._kp, self._kd
[docs] def SetMotorStrengthRatio(self, ratio):
"""Set the strength of all motors relative to the default value.
Args:
ratio: The relative strength. A scalar range from 0.0 to 1.0.
"""
if self._accurate_motor_model_enabled:
self._motor_model.set_strength_ratios([ratio] * self.num_motors)
[docs] def SetMotorStrengthRatios(self, ratios):
"""Set the strength of each motor relative to the default value.
Args:
ratios: The relative strength. A numpy array ranging from 0.0 to 1.0.
"""
if self._accurate_motor_model_enabled:
self._motor_model.set_strength_ratios(ratios)
[docs] def SetTimeSteps(self, action_repeat, simulation_step):
"""Set the time steps of the control and simulation.
Args:
action_repeat: The number of simulation steps that the same action is
repeated.
simulation_step: The simulation time step.
"""
self.time_step = simulation_step
self._action_repeat = action_repeat
@property
def chassis_link_ids(self):
return self._chassis_link_ids