update minitaur gym env
This commit is contained in:
Erwin Coumans
@@ -1,142 +1,530 @@
|
||||
import pybullet as p
|
||||
"""This file implements the functionalities of a minitaur using pybullet.
|
||||
|
||||
"""
|
||||
import copy
|
||||
import math
|
||||
import numpy as np
|
||||
import motor
|
||||
import os
|
||||
|
||||
class Minitaur:
|
||||
def __init__(self, urdfRootPath=''):
|
||||
self.urdfRootPath = urdfRootPath
|
||||
self.reset()
|
||||
|
||||
def applyAction(self, motorCommands):
|
||||
motorCommandsWithDir = np.multiply(motorCommands, self.motorDir)
|
||||
for i in range(self.nMotors):
|
||||
self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i])
|
||||
|
||||
def getObservation(self):
|
||||
observation = []
|
||||
observation.extend(self.getMotorAngles().tolist())
|
||||
observation.extend(self.getMotorVelocities().tolist())
|
||||
observation.extend(self.getMotorTorques().tolist())
|
||||
observation.extend(list(self.getBaseOrientation()))
|
||||
observation.extend(list(self.getBasePosition()))
|
||||
return observation
|
||||
|
||||
def getActionDimension(self):
|
||||
return self.nMotors
|
||||
|
||||
def getObservationDimension(self):
|
||||
return len(self.getObservation())
|
||||
|
||||
def buildJointNameToIdDict(self):
|
||||
nJoints = p.getNumJoints(self.quadruped)
|
||||
self.jointNameToId = {}
|
||||
for i in range(nJoints):
|
||||
jointInfo = p.getJointInfo(self.quadruped, i)
|
||||
self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
|
||||
self.resetPose()
|
||||
for i in range(100):
|
||||
p.stepSimulation()
|
||||
|
||||
def buildMotorIdList(self):
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint'])
|
||||
INIT_POSITION = [0, 0, .2]
|
||||
INIT_ORIENTATION = [0, 0, 0, 1]
|
||||
KNEE_CONSTRAINT_POINT_RIGHT = [0, 0.005, 0.2]
|
||||
KNEE_CONSTRAINT_POINT_LEFT = [0, 0.01, 0.2]
|
||||
OVERHEAT_SHUTDOWN_TORQUE = 2.45
|
||||
OVERHEAT_SHUTDOWN_TIME = 1.0
|
||||
LEG_POSITION = ["front_left", "back_left", "front_right", "back_right"]
|
||||
MOTOR_NAMES = [
|
||||
"motor_front_leftL_joint", "motor_front_leftR_joint",
|
||||
"motor_back_leftL_joint", "motor_back_leftR_joint",
|
||||
"motor_front_rightL_joint", "motor_front_rightR_joint",
|
||||
"motor_back_rightL_joint", "motor_back_rightR_joint"
|
||||
]
|
||||
LEG_LINK_ID = [2, 3, 5, 6, 8, 9, 11, 12, 15, 16, 18, 19, 21, 22, 24, 25]
|
||||
MOTOR_LINK_ID = [1, 4, 7, 10, 14, 17, 20, 23]
|
||||
FOOT_LINK_ID = [3, 6, 9, 12, 16, 19, 22, 25]
|
||||
BASE_LINK_ID = -1
|
||||
|
||||
|
||||
def reset(self):
|
||||
self.quadruped = p.loadURDF("%s/quadruped/quadruped.urdf" % self.urdfRootPath,0,0,.3)
|
||||
self.kp = 1
|
||||
self.kd = 0.1
|
||||
self.maxForce = 3.5
|
||||
self.nMotors = 8
|
||||
self.motorIdList = []
|
||||
self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1]
|
||||
self.buildJointNameToIdDict()
|
||||
self.buildMotorIdList()
|
||||
class Minitaur(object):
|
||||
"""The minitaur class that simulates a quadruped robot from Ghost Robotics.
|
||||
|
||||
"""
|
||||
|
||||
def disableAllMotors(self):
|
||||
nJoints = p.getNumJoints(self.quadruped)
|
||||
for i in range(nJoints):
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0)
|
||||
def __init__(self,
|
||||
pybullet_client,
|
||||
urdf_root= os.path.join(os.path.dirname(__file__),"../data"),
|
||||
time_step=0.01,
|
||||
self_collision_enabled=False,
|
||||
motor_velocity_limit=np.inf,
|
||||
pd_control_enabled=False,
|
||||
accurate_motor_model_enabled=False,
|
||||
motor_kp=1.0,
|
||||
motor_kd=0.02,
|
||||
torque_control_enabled=False,
|
||||
motor_overheat_protection=False,
|
||||
on_rack=False,
|
||||
kd_for_pd_controllers=0.3):
|
||||
"""Constructs a minitaur and reset it to the initial states.
|
||||
|
||||
def setMotorAngleById(self, motorId, desiredAngle):
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
|
||||
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.
|
||||
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.
|
||||
motor_kp: proportional gain for the accurate motor model
|
||||
motor_kd: derivative gain for the acurate motor model
|
||||
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 minitaur.py for more
|
||||
details.
|
||||
on_rack: Whether to place the minitaur on rack. This is only used to debug
|
||||
the walking gait. In this mode, the minitaur's base is hanged midair so
|
||||
that its walking gait is clearer to visualize.
|
||||
kd_for_pd_controllers: kd value for the pd controllers of the motors.
|
||||
"""
|
||||
self.num_motors = 8
|
||||
self.num_legs = self.num_motors / 2
|
||||
self._pybullet_client = pybullet_client
|
||||
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 = [-1, -1, -1, -1, 1, 1, 1, 1]
|
||||
self._observed_motor_torques = np.zeros(self.num_motors)
|
||||
self._applied_motor_torques = np.zeros(self.num_motors)
|
||||
self._max_force = 3.5
|
||||
self._accurate_motor_model_enabled = accurate_motor_model_enabled
|
||||
self._torque_control_enabled = torque_control_enabled
|
||||
self._motor_overheat_protection = motor_overheat_protection
|
||||
self._on_rack = on_rack
|
||||
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.Reset()
|
||||
|
||||
def setMotorAngleByName(self, motorName, desiredAngle):
|
||||
self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
|
||||
def _RecordMassInfoFromURDF(self):
|
||||
self._base_mass_urdf = self._pybullet_client.getDynamicsInfo(
|
||||
self.quadruped, BASE_LINK_ID)[0]
|
||||
self._leg_masses_urdf = []
|
||||
self._leg_masses_urdf.append(
|
||||
self._pybullet_client.getDynamicsInfo(self.quadruped, LEG_LINK_ID[0])[
|
||||
0])
|
||||
self._leg_masses_urdf.append(
|
||||
self._pybullet_client.getDynamicsInfo(self.quadruped, MOTOR_LINK_ID[0])[
|
||||
0])
|
||||
|
||||
def _BuildJointNameToIdDict(self):
|
||||
num_joints = self._pybullet_client.getNumJoints(self.quadruped)
|
||||
self._joint_name_to_id = {}
|
||||
for i in xrange(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 resetPose(self):
|
||||
#right front leg
|
||||
self.disableAllMotors()
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
|
||||
self.setMotorAngleByName('motor_front_rightR_joint', 1.57)
|
||||
self.setMotorAngleByName('motor_front_rightL_joint',-1.57)
|
||||
def _BuildMotorIdList(self):
|
||||
self._motor_id_list = [
|
||||
self._joint_name_to_id[motor_name] for motor_name in MOTOR_NAMES
|
||||
]
|
||||
|
||||
#left front leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
|
||||
self.setMotorAngleByName('motor_front_leftR_joint', 1.57)
|
||||
self.setMotorAngleByName('motor_front_leftL_joint',-1.57)
|
||||
def Reset(self, reload_urdf=True):
|
||||
"""Reset the minitaur to its initial states.
|
||||
|
||||
#right back leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
|
||||
self.setMotorAngleByName('motor_back_rightR_joint', 1.57)
|
||||
self.setMotorAngleByName('motor_back_rightL_joint',-1.57)
|
||||
Args:
|
||||
reload_urdf: Whether to reload the urdf file. If not, Reset() just place
|
||||
the minitaur back to its starting position.
|
||||
"""
|
||||
if reload_urdf:
|
||||
if self._self_collision_enabled:
|
||||
self.quadruped = self._pybullet_client.loadURDF(
|
||||
"%s/quadruped/minitaur.urdf" % self._urdf_root,
|
||||
INIT_POSITION,
|
||||
flags=self._pybullet_client.URDF_USE_SELF_COLLISION)
|
||||
else:
|
||||
self.quadruped = self._pybullet_client.loadURDF(
|
||||
"%s/quadruped/minitaur.urdf" % self._urdf_root, INIT_POSITION)
|
||||
self._BuildJointNameToIdDict()
|
||||
self._BuildMotorIdList()
|
||||
self._RecordMassInfoFromURDF()
|
||||
self.ResetPose(add_constraint=True)
|
||||
if self._on_rack:
|
||||
self._pybullet_client.createConstraint(
|
||||
self.quadruped, -1, -1, -1, self._pybullet_client.JOINT_FIXED,
|
||||
[0, 0, 0], [0, 0, 0], [0, 0, 1])
|
||||
else:
|
||||
self._pybullet_client.resetBasePositionAndOrientation(
|
||||
self.quadruped, INIT_POSITION, INIT_ORIENTATION)
|
||||
self._pybullet_client.resetBaseVelocity(self.quadruped, [0, 0, 0],
|
||||
[0, 0, 0])
|
||||
self.ResetPose(add_constraint=False)
|
||||
|
||||
#left back leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
|
||||
self.setMotorAngleByName('motor_back_leftR_joint', 1.57)
|
||||
self.setMotorAngleByName('motor_back_leftL_joint',-1.57)
|
||||
self._overheat_counter = np.zeros(self.num_motors)
|
||||
self._motor_enabled_list = [True] * self.num_motors
|
||||
|
||||
def getBasePosition(self):
|
||||
position, orientation = p.getBasePositionAndOrientation(self.quadruped)
|
||||
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):
|
||||
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)
|
||||
|
||||
def _SetDesiredMotorAngleByName(self, motor_name, desired_angle):
|
||||
self._SetDesiredMotorAngleById(self._joint_name_to_id[motor_name],
|
||||
desired_angle)
|
||||
|
||||
def ResetPose(self, add_constraint):
|
||||
"""Reset the pose of the minitaur.
|
||||
|
||||
Args:
|
||||
add_constraint: Whether to add a constraint at the joints of two feet.
|
||||
"""
|
||||
for i in xrange(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
|
||||
half_pi = math.pi / 2.0
|
||||
knee_angle = -2.1834
|
||||
|
||||
leg_position = LEG_POSITION[leg_id]
|
||||
self._pybullet_client.resetJointState(
|
||||
self.quadruped,
|
||||
self._joint_name_to_id["motor_" + leg_position + "L_joint"],
|
||||
self._motor_direction[2 * leg_id] * half_pi,
|
||||
targetVelocity=0)
|
||||
self._pybullet_client.resetJointState(
|
||||
self.quadruped,
|
||||
self._joint_name_to_id["knee_" + leg_position + "L_link"],
|
||||
self._motor_direction[2 * leg_id] * knee_angle,
|
||||
targetVelocity=0)
|
||||
self._pybullet_client.resetJointState(
|
||||
self.quadruped,
|
||||
self._joint_name_to_id["motor_" + leg_position + "R_joint"],
|
||||
self._motor_direction[2 * leg_id + 1] * half_pi,
|
||||
targetVelocity=0)
|
||||
self._pybullet_client.resetJointState(
|
||||
self.quadruped,
|
||||
self._joint_name_to_id["knee_" + leg_position + "R_link"],
|
||||
self._motor_direction[2 * leg_id + 1] * knee_angle,
|
||||
targetVelocity=0)
|
||||
if add_constraint:
|
||||
self._pybullet_client.createConstraint(
|
||||
self.quadruped, self._joint_name_to_id["knee_"
|
||||
+ leg_position + "R_link"],
|
||||
self.quadruped, self._joint_name_to_id["knee_"
|
||||
+ leg_position + "L_link"],
|
||||
self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0],
|
||||
KNEE_CONSTRAINT_POINT_RIGHT, KNEE_CONSTRAINT_POINT_LEFT)
|
||||
|
||||
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 + "L_joint"]),
|
||||
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 + "R_joint"]),
|
||||
controlMode=self._pybullet_client.VELOCITY_CONTROL,
|
||||
targetVelocity=0,
|
||||
force=knee_friction_force)
|
||||
|
||||
else:
|
||||
self._SetDesiredMotorAngleByName(
|
||||
"motor_" + leg_position + "L_joint",
|
||||
self._motor_direction[2 * leg_id] * half_pi)
|
||||
self._SetDesiredMotorAngleByName("motor_" + leg_position + "R_joint",
|
||||
self._motor_direction[2 * leg_id
|
||||
+ 1] * half_pi)
|
||||
|
||||
self._pybullet_client.setJointMotorControl2(
|
||||
bodyIndex=self.quadruped,
|
||||
jointIndex=(self._joint_name_to_id["knee_" + leg_position + "L_link"]),
|
||||
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["knee_" + leg_position + "R_link"]),
|
||||
controlMode=self._pybullet_client.VELOCITY_CONTROL,
|
||||
targetVelocity=0,
|
||||
force=knee_friction_force)
|
||||
|
||||
def GetBasePosition(self):
|
||||
"""Get the position of minitaur's base.
|
||||
|
||||
Returns:
|
||||
The position of minitaur's base.
|
||||
"""
|
||||
position, _ = (
|
||||
self._pybullet_client.getBasePositionAndOrientation(self.quadruped))
|
||||
return position
|
||||
|
||||
def getBaseOrientation(self):
|
||||
position, orientation = p.getBasePositionAndOrientation(self.quadruped)
|
||||
def GetBaseOrientation(self):
|
||||
"""Get the orientation of minitaur's base, represented as quaternion.
|
||||
|
||||
Returns:
|
||||
The orientation of minitaur's base.
|
||||
"""
|
||||
_, orientation = (
|
||||
self._pybullet_client.getBasePositionAndOrientation(self.quadruped))
|
||||
return orientation
|
||||
|
||||
def getMotorAngles(self):
|
||||
motorAngles = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorAngles.append(jointState[0])
|
||||
motorAngles = np.multiply(motorAngles, self.motorDir)
|
||||
return motorAngles
|
||||
def GetActionDimension(self):
|
||||
"""Get the length of the action list.
|
||||
|
||||
def getMotorVelocities(self):
|
||||
motorVelocities = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorVelocities.append(jointState[1])
|
||||
motorVelocities = np.multiply(motorVelocities, self.motorDir)
|
||||
return motorVelocities
|
||||
Returns:
|
||||
The length of the action list.
|
||||
"""
|
||||
return self.num_motors
|
||||
|
||||
def getMotorTorques(self):
|
||||
motorTorques = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorTorques.append(jointState[3])
|
||||
motorTorques = np.multiply(motorTorques, self.motorDir)
|
||||
return motorTorques
|
||||
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.
|
||||
"""
|
||||
upper_bound = np.array([0.0] * self.GetObservationDimension())
|
||||
upper_bound[0:self.num_motors] = math.pi # Joint angle.
|
||||
upper_bound[self.num_motors:2 * self.num_motors] = (
|
||||
motor.MOTOR_SPEED_LIMIT) # Joint velocity.
|
||||
upper_bound[2 * self.num_motors:3 * self.num_motors] = (
|
||||
motor.OBSERVED_TORQUE_LIMIT) # Joint torque.
|
||||
upper_bound[3 * self.num_motors:] = 1.0 # Quaternion of base orientation.
|
||||
return upper_bound
|
||||
|
||||
def GetObservationLowerBound(self):
|
||||
"""Get the lower bound of the observation."""
|
||||
return -self.GetObservationUpperBound()
|
||||
|
||||
def GetObservationDimension(self):
|
||||
"""Get the length of the observation list.
|
||||
|
||||
Returns:
|
||||
The length of the observation list.
|
||||
"""
|
||||
return len(self.GetObservation())
|
||||
|
||||
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.
|
||||
"""
|
||||
observation = []
|
||||
observation.extend(self.GetMotorAngles().tolist())
|
||||
observation.extend(self.GetMotorVelocities().tolist())
|
||||
observation.extend(self.GetMotorTorques().tolist())
|
||||
observation.extend(list(self.GetBaseOrientation()))
|
||||
return observation
|
||||
|
||||
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.
|
||||
"""
|
||||
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 xrange(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)
|
||||
|
||||
def GetMotorAngles(self):
|
||||
"""Get the eight motor angles at the current moment.
|
||||
|
||||
Returns:
|
||||
Motor angles.
|
||||
"""
|
||||
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 motor_angles
|
||||
|
||||
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
|
||||
|
||||
def GetMotorTorques(self):
|
||||
"""Get the amount of torques 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
|
||||
|
||||
def ConvertFromLegModel(self, actions):
|
||||
"""Convert the actions that use leg model to the real motor actions.
|
||||
|
||||
Args:
|
||||
actions: The theta, phi of the leg model.
|
||||
Returns:
|
||||
The eight desired motor angles that can be used in ApplyActions().
|
||||
"""
|
||||
|
||||
motor_angle = copy.deepcopy(actions)
|
||||
scale_for_singularity = 1
|
||||
offset_for_singularity = 1.5
|
||||
half_num_motors = self.num_motors / 2
|
||||
quater_pi = math.pi / 4
|
||||
for i in xrange(self.num_motors):
|
||||
action_idx = i // 2
|
||||
forward_backward_component = (-scale_for_singularity * quater_pi * (
|
||||
actions[action_idx + half_num_motors] + offset_for_singularity))
|
||||
extension_component = (-1)**i * quater_pi * actions[action_idx]
|
||||
if i >= half_num_motors:
|
||||
extension_component = -extension_component
|
||||
motor_angle[i] = (
|
||||
math.pi + forward_backward_component + extension_component)
|
||||
return motor_angle
|
||||
|
||||
def GetBaseMassFromURDF(self):
|
||||
"""Get the mass of the base from the URDF file."""
|
||||
return self._base_mass_urdf
|
||||
|
||||
def GetLegMassesFromURDF(self):
|
||||
"""Get the mass of the legs from the URDF file."""
|
||||
return self._leg_masses_urdf
|
||||
|
||||
def SetBaseMass(self, base_mass):
|
||||
self._pybullet_client.changeDynamics(
|
||||
self.quadruped, BASE_LINK_ID, mass=base_mass)
|
||||
|
||||
def SetLegMasses(self, leg_masses):
|
||||
"""Set the mass of the legs.
|
||||
|
||||
A leg includes leg_link and motor. All four leg_links have the same mass,
|
||||
which is leg_masses[0]. All four motors have the same mass, which is
|
||||
leg_mass[1].
|
||||
|
||||
Args:
|
||||
leg_masses: The leg masses. leg_masses[0] is the mass of the leg link.
|
||||
leg_masses[1] is the mass of the motor.
|
||||
"""
|
||||
for link_id in LEG_LINK_ID:
|
||||
self._pybullet_client.changeDynamics(
|
||||
self.quadruped, link_id, mass=leg_masses[0])
|
||||
for link_id in MOTOR_LINK_ID:
|
||||
self._pybullet_client.changeDynamics(
|
||||
self.quadruped, link_id, mass=leg_masses[1])
|
||||
|
||||
def SetFootFriction(self, foot_friction):
|
||||
"""Set the lateral friction of the feet.
|
||||
|
||||
Args:
|
||||
foot_friction: The lateral friction coefficient of the foot. This value is
|
||||
shared by all four feet.
|
||||
"""
|
||||
for link_id in FOOT_LINK_ID:
|
||||
self._pybullet_client.changeDynamics(
|
||||
self.quadruped, link_id, lateralFriction=foot_friction)
|
||||
|
||||
def SetBatteryVoltage(self, voltage):
|
||||
if self._accurate_motor_model_enabled:
|
||||
self._motor_model.set_voltage(voltage)
|
||||
|
||||
def SetMotorViscousDamping(self, viscous_damping):
|
||||
if self._accurate_motor_model_enabled:
|
||||
self._motor_model.set_viscous_damping(viscous_damping)
|
||||
|
||||
Reference in New Issue
Block a user