From 0851b45f39053d4e6798ecc04de24e026a1d61c1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 23 Aug 2017 17:41:34 -0700 Subject: [PATCH] update minitaur gym env --- .../gym/pybullet_envs/bullet/bullet_client.py | 29 + .../gym/pybullet_envs/bullet/minitaur.py | 632 ++++++++++++++---- .../pybullet_envs/bullet/minitaurGymEnv.py | 113 ---- .../pybullet_envs/bullet/minitaur_bullet.py | 90 --- .../pybullet_envs/bullet/minitaur_gym_env.py | 346 ++++++++++ .../gym/pybullet_envs/bullet/minitaur_new.py | 176 ----- .../gym/pybullet_envs/bullet/motor.py | 101 +++ 7 files changed, 986 insertions(+), 501 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py delete mode 100644 examples/pybullet/gym/pybullet_envs/bullet/minitaurGymEnv.py delete mode 100644 examples/pybullet/gym/pybullet_envs/bullet/minitaur_bullet.py create mode 100644 examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py delete mode 100644 examples/pybullet/gym/pybullet_envs/bullet/minitaur_new.py create mode 100644 examples/pybullet/gym/pybullet_envs/bullet/motor.py diff --git a/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py b/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py new file mode 100644 index 000000000..3c09f85e3 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py @@ -0,0 +1,29 @@ +import functools +import inspect +import pybullet + + +class BulletClient(object): + """A wrapper for pybullet to manage different clients.""" + + def __init__(self, connection_mode=pybullet.DIRECT): + """Create a simulation and connect to it.""" + self._client = pybullet.connect(connection_mode) + self._shapes = {} + + def __del__(self): + """Clean up connection if not already done.""" + try: + pybullet.disconnect(physicsClientId=self._client) + except pybullet.error: + pass + + def __getattr__(self, name): + """Inject the client id into Bullet functions.""" + attribute = getattr(pybullet, name) + if inspect.isbuiltin(attribute): + if name not in ["invertTransform", "multiplyTransforms", + "getMatrixFromQuaternion"]: # A temporary hack for now. + attribute = functools.partial(attribute, physicsClientId=self._client) + return attribute + diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py index 74ddedf84..252f3f906 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py @@ -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) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaurGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaurGymEnv.py deleted file mode 100644 index d70a398bd..000000000 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaurGymEnv.py +++ /dev/null @@ -1,113 +0,0 @@ -import math -import gym -from gym import spaces -from gym.utils import seeding -import numpy as np -import time -import pybullet as p -from . import minitaur_new - -class MinitaurGymEnv(gym.Env): - metadata = { - 'render.modes': ['human', 'rgb_array'], - 'video.frames_per_second' : 50 - } - - def __init__(self, - urdfRoot="", - actionRepeat=1, - isEnableSelfCollision=True, - motorVelocityLimit=10.0, - render=False): - self._timeStep = 0.01 - self._urdfRoot = urdfRoot - self._actionRepeat = actionRepeat - self._motorVelocityLimit = motorVelocityLimit - self._isEnableSelfCollision = isEnableSelfCollision - self._observation = [] - self._envStepCounter = 0 - self._render = render - self._lastBasePosition = [0, 0, 0] - if self._render: - p.connect(p.GUI) - else: - p.connect(p.DIRECT) - self._seed() - self.reset() - observationDim = self._minitaur.getObservationDimension() - observation_high = np.array([np.finfo(np.float32).max] * observationDim) - actionDim = 8 - action_high = np.array([1] * actionDim) - self.action_space = spaces.Box(-action_high, action_high) - self.observation_space = spaces.Box(-observation_high, observation_high) - self.viewer = None - - def _reset(self): - p.resetSimulation() - p.setPhysicsEngineParameter(numSolverIterations=300) - p.setTimeStep(self._timeStep) - p.loadURDF("%splane.urdf" % self._urdfRoot) - p.setGravity(0,0,-10) - self._minitaur = minitaur_new.Minitaur(urdfRootPath=self._urdfRoot, timeStep=self._timeStep, isEnableSelfCollision=self._isEnableSelfCollision, motorVelocityLimit=self._motorVelocityLimit) - self._envStepCounter = 0 - self._lastBasePosition = [0, 0, 0] - for i in range(100): - p.stepSimulation() - self._observation = self._minitaur.getObservation() - return self._observation - - def __del__(self): - p.disconnect() - - def _seed(self, seed=None): - self.np_random, seed = seeding.np_random(seed) - return [seed] - - def _step(self, action): - if (self._render): - basePos = self._minitaur.getBasePosition() - p.resetDebugVisualizerCamera(1, 30, 40, basePos) - - if len(action) != self._minitaur.getActionDimension(): - raise ValueError("We expect {} continuous action not {}.".format(self._minitaur.getActionDimension(), len(action))) - - for i in range(len(action)): - if not -1.01 <= action[i] <= 1.01: - raise ValueError("{}th action should be between -1 and 1 not {}.".format(i, action[i])) - - realAction = self._minitaur.convertFromLegModel(action) - self._minitaur.applyAction(realAction) - for i in range(self._actionRepeat): - p.stepSimulation() - if self._render: - time.sleep(self._timeStep) - self._observation = self._minitaur.getObservation() - if self._termination(): - break - self._envStepCounter += 1 - reward = self._reward() - done = self._termination() - return np.array(self._observation), reward, done, {} - - def _render(self, mode='human', close=False): - return - - def is_fallen(self): - orientation = self._minitaur.getBaseOrientation() - rotMat = p.getMatrixFromQuaternion(orientation) - localUp = rotMat[6:] - return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0 or self._observation[-1] < 0.1 - - def _termination(self): - return self.is_fallen() - - def _reward(self): - currentBasePosition = self._minitaur.getBasePosition() - forward_reward = currentBasePosition[0] - self._lastBasePosition[0] - self._lastBasePosition = currentBasePosition - - energyWeight = 0.001 - energy = np.abs(np.dot(self._minitaur.getMotorTorques(), self._minitaur.getMotorVelocities())) * self._timeStep - energy_reward = energyWeight * energy - reward = forward_reward - energy_reward - return reward diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_bullet.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_bullet.py deleted file mode 100644 index 7e52da29e..000000000 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_bullet.py +++ /dev/null @@ -1,90 +0,0 @@ -import math -import gym -from gym import spaces -from gym.utils import seeding -import numpy as np -import time - -import pybullet as p -from pybullet_envs.bullet.minitaur import Minitaur - -class MinitaurBulletEnv(gym.Env): - metadata = { - 'render.modes': ['human', 'rgb_array'], - 'video.frames_per_second' : 50 - } - - def __init__(self): - self._timeStep = 0.01 - self._observation = [] - self._envStepCounter = 0 - self._lastBasePosition = [0, 0, 0] - - p.connect(p.GUI) - - p.resetSimulation() - p.setTimeStep(self._timeStep) - p.loadURDF("plane.urdf") - p.setGravity(0,0,-10) - self._minitaur = Minitaur() - - observationDim = self._minitaur.getObservationDimension() - observation_high = np.array([np.finfo(np.float32).max] * observationDim) - actionDim = 8 - action_high = np.array([math.pi / 2.0] * actionDim) - self.action_space = spaces.Box(-action_high, action_high) - self.observation_space = spaces.Box(-observation_high, observation_high) - - self._seed() - self.reset() - self.viewer = None - - def __del__(self): - p.disconnect() - - def _seed(self, seed=None): - self.np_random, seed = seeding.np_random(seed) - return [seed] - - def _step(self, action): - if len(action) != self._minitaur.getActionDimension(): - raise ValueError("We expect {} continuous action not {}.".format(self._minitaur.getActionDimension(), len(actions.continuous_actions))) - - for i in range(len(action)): - if not -math.pi/2 <= action[i] <= math.pi/2: - raise ValueError("{}th action should be between -1 and 1 not {}.".format(i, action[i])) - action[i] += math.pi / 2 - - self._minitaur.applyAction(action) - p.stepSimulation() - self._observation = self._minitaur.getObservation() - self._envStepCounter += 1 - reward = self._reward() - done = self._termination() - return np.array(self._observation), reward, done, {} - - def _reset(self): - p.resetSimulation() - p.setTimeStep(self._timeStep) - p.loadURDF("plane.urdf") - p.setGravity(0,0,-10) - self._minitaur = Minitaur() - self._observation = self._minitaur.getObservation() - - def _render(self, mode='human', close=False): - return - - def is_fallen(self): - orientation = self._minitaur.getBaseOrientation() - rotMat = p.getMatrixFromQuaternion(orientation) - localUp = rotMat[6:] - return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0 - - def _termination(self): - return self.is_fallen() - - def _reward(self): - currentBasePosition = self._minitaur.getBasePosition() - reward = np.dot(np.asarray([-1, 0, 0]), np.asarray(currentBasePosition) - np.asarray(self._lastBasePosition)) - self._lastBasePosition = currentBasePosition - return reward diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py new file mode 100644 index 000000000..b1c020434 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py @@ -0,0 +1,346 @@ +"""This file implements the gym environment of minitaur. + +""" +import math +import time +import gym +from gym import spaces +from gym.utils import seeding +import numpy as np +import pybullet +import bullet_client +import minitaur +import os + +NUM_SUBSTEPS = 5 +NUM_MOTORS = 8 +MOTOR_ANGLE_OBSERVATION_INDEX = 0 +MOTOR_VELOCITY_OBSERVATION_INDEX = MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS +MOTOR_TORQUE_OBSERVATION_INDEX = MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS +BASE_ORIENTATION_OBSERVATION_INDEX = MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS +ACTION_EPS = 0.01 +OBSERVATION_EPS = 0.01 + + +class MinitaurGymEnv(gym.Env): + """The gym environment for the minitaur. + + It simulates the locomotion of a minitaur, a quadruped robot. The state space + include the angles, velocities and torques for all the motors and the action + space is the desired motor angle for each motor. The reward function is based + on how far the minitaur walks in 1000 steps and penalizes the energy + expenditure. + + """ + metadata = { + "render.modes": ["human", "rgb_array"], + "video.frames_per_second": 50 + } + + def __init__(self, + urdf_root=os.path.join(os.path.dirname(__file__),"../data"), + action_repeat=1, + distance_weight=1.0, + energy_weight=0.005, + shake_weight=0.0, + drift_weight=0.0, + distance_limit=float("inf"), + observation_noise_stdev=0.0, + self_collision_enabled=True, + motor_velocity_limit=np.inf, + pd_control_enabled=False, + leg_model_enabled=True, + accurate_motor_model_enabled=False, + motor_kp=1.0, + motor_kd=0.02, + torque_control_enabled=False, + motor_overheat_protection=False, + hard_reset=True, + on_rack=False, + render=False, + kd_for_pd_controllers=0.3, + env_randomizer=None): + """Initialize the minitaur gym environment. + + Args: + urdf_root: The path to the urdf data folder. + action_repeat: The number of simulation steps before actions are applied. + distance_weight: The weight of the distance term in the reward. + energy_weight: The weight of the energy term in the reward. + shake_weight: The weight of the vertical shakiness term in the reward. + drift_weight: The weight of the sideways drift term in the reward. + distance_limit: The maximum distance to terminate the episode. + observation_noise_stdev: The standard deviation of observation noise. + self_collision_enabled: Whether to enable self collision in the sim. + motor_velocity_limit: The velocity limit of each motor. + pd_control_enabled: Whether to use PD controller for each motor. + leg_model_enabled: Whether to use a leg motor to reparameterize the action + space. + 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 accurate 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. + hard_reset: Whether to wipe the simulation and load everything when reset + is called. If set to false, reset just place the minitaur back to start + position and set its pose to initial configuration. + 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. + render: Whether to render the simulation. + kd_for_pd_controllers: kd value for the pd controllers of the motors + env_randomizer: An EnvRandomizer to randomize the physical properties + during reset(). + """ + self._time_step = 0.01 + self._action_repeat = action_repeat + self._num_bullet_solver_iterations = 300 + self._urdf_root = urdf_root + self._self_collision_enabled = self_collision_enabled + self._motor_velocity_limit = motor_velocity_limit + self._observation = [] + self._env_step_counter = 0 + self._is_render = render + self._last_base_position = [0, 0, 0] + self._distance_weight = distance_weight + self._energy_weight = energy_weight + self._drift_weight = drift_weight + self._shake_weight = shake_weight + self._distance_limit = distance_limit + self._observation_noise_stdev = observation_noise_stdev + self._action_bound = 1 + self._pd_control_enabled = pd_control_enabled + self._leg_model_enabled = leg_model_enabled + self._accurate_motor_model_enabled = accurate_motor_model_enabled + self._motor_kp = motor_kp + self._motor_kd = motor_kd + self._torque_control_enabled = torque_control_enabled + self._motor_overheat_protection = motor_overheat_protection + self._on_rack = on_rack + self._cam_dist = 1.0 + self._cam_yaw = 0 + self._cam_pitch = -30 + self._hard_reset = True + self._kd_for_pd_controllers = kd_for_pd_controllers + self._last_frame_time = 0.0 + self._env_randomizer = env_randomizer + # PD control needs smaller time step for stability. + if pd_control_enabled or accurate_motor_model_enabled: + self._time_step /= NUM_SUBSTEPS + self._num_bullet_solver_iterations /= NUM_SUBSTEPS + self._action_repeat *= NUM_SUBSTEPS + + if self._is_render: + self._pybullet_client = bullet_client.BulletClient( + connection_mode=pybullet.GUI) + else: + self._pybullet_client = bullet_client.BulletClient() + + self._seed() + self.reset() + observation_high = ( + self.minitaur.GetObservationUpperBound() + OBSERVATION_EPS) + observation_low = ( + self.minitaur.GetObservationLowerBound() - OBSERVATION_EPS) + action_dim = 8 + action_high = np.array([self._action_bound] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) + self.observation_space = spaces.Box(observation_low, observation_high) + self.viewer = None + self._hard_reset = hard_reset # This assignment need to be after reset() + + def set_env_randomizer(self, env_randomizer): + self._env_randomizer = env_randomizer + + def _reset(self): + if self._hard_reset: + self._pybullet_client.resetSimulation() + self._pybullet_client.setPhysicsEngineParameter( + numSolverIterations=self._num_bullet_solver_iterations) + self._pybullet_client.setTimeStep(self._time_step) + self._pybullet_client.loadURDF("%s/plane.urdf" % self._urdf_root) + self._pybullet_client.setGravity(0, 0, -10) + acc_motor = self._accurate_motor_model_enabled + motor_protect = self._motor_overheat_protection + self.minitaur = (minitaur.Minitaur( + pybullet_client=self._pybullet_client, + urdf_root=self._urdf_root, + time_step=self._time_step, + self_collision_enabled=self._self_collision_enabled, + motor_velocity_limit=self._motor_velocity_limit, + pd_control_enabled=self._pd_control_enabled, + accurate_motor_model_enabled=acc_motor, + motor_kp=self._motor_kp, + motor_kd=self._motor_kd, + torque_control_enabled=self._torque_control_enabled, + motor_overheat_protection=motor_protect, + on_rack=self._on_rack, + kd_for_pd_controllers=self._kd_for_pd_controllers)) + else: + self.minitaur.Reset(reload_urdf=False) + + if self._env_randomizer is not None: + self._env_randomizer.randomize_env(self) + + self._env_step_counter = 0 + self._last_base_position = [0, 0, 0] + self._objectives = [] + self._pybullet_client.resetDebugVisualizerCamera( + self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0]) + if not self._torque_control_enabled: + for _ in xrange(100): + if self._pd_control_enabled or self._accurate_motor_model_enabled: + self.minitaur.ApplyAction([math.pi / 2] * 8) + self._pybullet_client.stepSimulation() + return self._noisy_observation() + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def _transform_action_to_motor_command(self, action): + if self._leg_model_enabled: + for i, action_component in enumerate(action): + if not (-self._action_bound - ACTION_EPS <= action_component <= + self._action_bound + ACTION_EPS): + raise ValueError( + "{}th action {} out of bounds.".format(i, action_component)) + action = self.minitaur.ConvertFromLegModel(action) + return action + + def _step(self, action): + """Step forward the simulation, given the action. + + Args: + action: A list of desired motor angles for eight motors. + + Returns: + observations: The angles, velocities and torques of all motors. + reward: The reward for the current state-action pair. + done: Whether the episode has ended. + info: A dictionary that stores diagnostic information. + + Raises: + ValueError: The action dimension is not the same as the number of motors. + ValueError: The magnitude of actions is out of bounds. + """ + if self._is_render: + # Sleep, otherwise the computation takes less time than real time, + # which will make the visualization like a fast-forward video. + time_spent = time.time() - self._last_frame_time + self._last_frame_time = time.time() + time_to_sleep = self._action_repeat * self._time_step - time_spent + if time_to_sleep > 0: + time.sleep(time_to_sleep) + base_pos = self.minitaur.GetBasePosition() + self._pybullet_client.resetDebugVisualizerCamera( + self._cam_dist, self._cam_yaw, self._cam_pitch, base_pos) + action = self._transform_action_to_motor_command(action) + for _ in xrange(self._action_repeat): + self.minitaur.ApplyAction(action) + self._pybullet_client.stepSimulation() + + self._env_step_counter += 1 + reward = self._reward() + done = self._termination() + return np.array(self._noisy_observation()), reward, done, {} + + def _render(self, mode="human", close=False): + return + + def get_minitaur_motor_angles(self): + """Get the minitaur's motor angles. + + Returns: + A numpy array of motor angles. + """ + return np.array( + self._observation[MOTOR_ANGLE_OBSERVATION_INDEX: + MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_motor_velocities(self): + """Get the minitaur's motor velocities. + + Returns: + A numpy array of motor velocities. + """ + return np.array( + self._observation[MOTOR_VELOCITY_OBSERVATION_INDEX: + MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_motor_torques(self): + """Get the minitaur's motor torques. + + Returns: + A numpy array of motor torques. + """ + return np.array( + self._observation[MOTOR_TORQUE_OBSERVATION_INDEX: + MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_base_orientation(self): + """Get the minitaur's base orientation, represented by a quaternion. + + Returns: + A numpy array of minitaur's orientation. + """ + return np.array(self._observation[BASE_ORIENTATION_OBSERVATION_INDEX:]) + + def is_fallen(self): + """Decide whether the minitaur has fallen. + + If the up directions between the base and the world is larger (the dot + product is smaller than 0.85) or the base is very low on the ground + (the height is smaller than 0.13 meter), the minitaur is considered fallen. + + Returns: + Boolean value that indicates whether the minitaur has fallen. + """ + orientation = self.minitaur.GetBaseOrientation() + rot_mat = self._pybullet_client.getMatrixFromQuaternion(orientation) + local_up = rot_mat[6:] + pos = self.minitaur.GetBasePosition() + return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.85 or + pos[2] < 0.13) + + def _termination(self): + position = self.minitaur.GetBasePosition() + distance = math.sqrt(position[0]**2 + position[1]**2) + return self.is_fallen() or distance > self._distance_limit + + def _reward(self): + current_base_position = self.minitaur.GetBasePosition() + forward_reward = current_base_position[0] - self._last_base_position[0] + drift_reward = -abs(current_base_position[1] - self._last_base_position[1]) + shake_reward = -abs(current_base_position[2] - self._last_base_position[2]) + self._last_base_position = current_base_position + energy_reward = np.abs( + np.dot(self.minitaur.GetMotorTorques(), + self.minitaur.GetMotorVelocities())) * self._time_step + reward = ( + self._distance_weight * forward_reward - + self._energy_weight * energy_reward + self._drift_weight * drift_reward + + self._shake_weight * shake_reward) + self._objectives.append( + [forward_reward, energy_reward, drift_reward, shake_reward]) + return reward + + def get_objectives(self): + return self._objectives + + def _get_observation(self): + self._observation = self.minitaur.GetObservation() + return self._observation + + def _noisy_observation(self): + self._get_observation() + observation = np.array(self._observation) + if self._observation_noise_stdev > 0: + observation += (np.random.normal( + scale=self._observation_noise_stdev, size=observation.shape) * + self.minitaur.GetObservationUpperBound()) + return observation diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_new.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_new.py deleted file mode 100644 index e23fd7dca..000000000 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_new.py +++ /dev/null @@ -1,176 +0,0 @@ -import pybullet as p -import numpy as np -import copy -import math - -class Minitaur: - - def __init__(self, urdfRootPath='', timeStep=0.01, isEnableSelfCollision=True, motorVelocityLimit=10.0): - self.urdfRootPath = urdfRootPath - self.isEnableSelfCollision = isEnableSelfCollision - self.motorVelocityLimit = motorVelocityLimit - self.timeStep = timeStep - self.reset() - - 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() - - def buildMotorIdList(self): - self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint']) - self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint']) - self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint']) - self.motorIdList.append(self.jointNameToId['motor_back_leftR_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']) - - def reset(self): - if self.isEnableSelfCollision: - self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2], flags=p.URDF_USE_SELF_COLLISION) - else: - self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2]) - self.kp = 1 - self.kd = 1 - self.maxForce = 3.5 - self.nMotors = 8 - self.motorIdList = [] - self.motorDir = [-1, -1, -1, -1, 1, 1, 1, 1] - self.buildJointNameToIdDict() - self.buildMotorIdList() - - - 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) - - def setMotorAngleByName(self, motorName, desiredAngle): - self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle) - - def resetPose(self): - kneeFrictionForce = 0 - halfpi = 1.57079632679 - kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length) - - #left front leg - p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi) - p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle) - p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi) - p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle) - 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.005,0.2],[0,0.01,0.2]) - self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi) - self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi) - p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) - p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) - - #left back leg - p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi) - p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle) - p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi) - p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle) - 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.005,0.2],[0,0.01,0.2]) - self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi) - self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi) - p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) - p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) - - #right front leg - p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi) - p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle) - p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi) - p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle) - 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.005,0.2],[0,0.01,0.2]) - self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi) - self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi) - p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) - p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) - - - #right back leg - p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi) - p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle) - p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi) - p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle) - 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.005,0.2],[0,0.01,0.2]) - self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi) - self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi) - p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) - p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) - - - def getBasePosition(self): - position, orientation = p.getBasePositionAndOrientation(self.quadruped) - return position - - def getBaseOrientation(self): - position, orientation = p.getBasePositionAndOrientation(self.quadruped) - return orientation - - def getActionDimension(self): - return self.nMotors - - def getObservationDimension(self): - return len(self.getObservation()) - - def getObservation(self): - 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, motorCommands): - if self.motorVelocityLimit < np.inf: - currentMotorAngle = self.getMotorAngles() - motorCommandsMax = currentMotorAngle + self.timeStep * self.motorVelocityLimit - motorCommandsMin = currentMotorAngle - self.timeStep * self.motorVelocityLimit - motorCommands = np.clip(motorCommands, motorCommandsMin, motorCommandsMax) - motorCommandsWithDir = np.multiply(motorCommands, self.motorDir) -# print('action: {}'.format(motorCommands)) -# print('motor: {}'.format(motorCommandsWithDir)) - for i in range(self.nMotors): - self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i]) - - 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 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 - - 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 convertFromLegModel(self, actions): - motorAngle = copy.deepcopy(actions) - scaleForSingularity = 1 - offsetForSingularity = 0.5 - motorAngle[0] = math.pi + math.pi / 4 * actions[0] - scaleForSingularity * math.pi / 4 * (actions[4] + 1 + offsetForSingularity) - motorAngle[1] = math.pi - math.pi / 4 * actions[0] - scaleForSingularity * math.pi / 4 * (actions[4] + 1 + offsetForSingularity) - motorAngle[2] = math.pi + math.pi / 4 * actions[1] - scaleForSingularity * math.pi / 4 * (actions[5] + 1 + offsetForSingularity) - motorAngle[3] = math.pi - math.pi / 4 * actions[1] - scaleForSingularity * math.pi / 4 * (actions[5] + 1 + offsetForSingularity) - motorAngle[4] = math.pi - math.pi / 4 * actions[2] - scaleForSingularity * math.pi / 4 * (actions[6] + 1 + offsetForSingularity) - motorAngle[5] = math.pi + math.pi / 4 * actions[2] - scaleForSingularity * math.pi / 4 * (actions[6] + 1 + offsetForSingularity) - motorAngle[6] = math.pi - math.pi / 4 * actions[3] - scaleForSingularity * math.pi / 4 * (actions[7] + 1 + offsetForSingularity) - motorAngle[7] = math.pi + math.pi / 4 * actions[3] - scaleForSingularity * math.pi / 4 * (actions[7] + 1 + offsetForSingularity) - return motorAngle diff --git a/examples/pybullet/gym/pybullet_envs/bullet/motor.py b/examples/pybullet/gym/pybullet_envs/bullet/motor.py new file mode 100644 index 000000000..fa6b4f2da --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/bullet/motor.py @@ -0,0 +1,101 @@ +"""This file implements an accurate motor model.""" +import numpy as np + +VOLTAGE_CLIPPING = 50 +OBSERVED_TORQUE_LIMIT = 5.7 +MOTOR_VOLTAGE = 16.0 +MOTOR_RESISTANCE = 0.186 +MOTOR_TORQUE_CONSTANT = 0.0954 +MOTOR_VISCOUS_DAMPING = 0 +MOTOR_SPEED_LIMIT = MOTOR_VOLTAGE / (MOTOR_VISCOUS_DAMPING + + MOTOR_TORQUE_CONSTANT) + + +class MotorModel(object): + """The accurate motor model, which is based on the physics of DC motors. + + The motor model support two types of control: position control and torque + control. In position control mode, a desired motor angle is specified, and a + torque is computed based on the internal motor model. When the torque control + is specified, a pwm signal in the range of [-1.0, 1.0] is converted to the + torque. + + The internal motor model takes the following factors into consideration: + pd gains, viscous friction, back-EMF voltage and current-torque profile. + """ + + def __init__(self, + torque_control_enabled=False, + kp=1.2, + kd=0): + self._torque_control_enabled = torque_control_enabled + self._kp = kp + self._kd = kd + self._resistance = MOTOR_RESISTANCE + self._voltage = MOTOR_VOLTAGE + self._torque_constant = MOTOR_TORQUE_CONSTANT + self._viscous_damping = MOTOR_VISCOUS_DAMPING + self._current_table = [0, 10, 20, 30, 40, 50, 60] + self._torque_table = [0, 1, 1.9, 2.45, 3.0, 3.25, 3.5] + + def set_voltage(self, voltage): + self._voltage = voltage + + def get_voltage(self): + return self._voltage + + def set_viscous_damping(self, viscous_damping): + self._viscous_damping = viscous_damping + + def get_viscous_dampling(self): + return self._viscous_damping + + def convert_to_torque(self, motor_commands, current_motor_angle, + current_motor_velocity): + """Convert the commands (position control or torque control) to torque. + + Args: + motor_commands: The desired motor angle if the motor is in position + control mode. The pwm signal if the motor is in torque control mode. + current_motor_angle: The motor angle at the current time step. + current_motor_velocity: The motor velocity at the current time step. + Returns: + actual_torque: The torque that needs to be applied to the motor. + observed_torque: The torque observed by the sensor. + """ + if self._torque_control_enabled: + pwm = motor_commands + else: + pwm = (-self._kp * (current_motor_angle - motor_commands) + - self._kd * current_motor_velocity) + pwm = np.clip(pwm, -1.0, 1.0) + return self._convert_to_torque_from_pwm(pwm, current_motor_velocity) + + def _convert_to_torque_from_pwm(self, pwm, current_motor_velocity): + """Convert the pwm signal to torque. + + Args: + pwm: The pulse width modulation. + current_motor_velocity: The motor velocity at the current time step. + Returns: + actual_torque: The torque that needs to be applied to the motor. + observed_torque: The torque observed by the sensor. + """ + observed_torque = np.clip( + self._torque_constant * (pwm * self._voltage / self._resistance), + -OBSERVED_TORQUE_LIMIT, OBSERVED_TORQUE_LIMIT) + + # Net voltage is clipped at 50V by diodes on the motor controller. + voltage_net = np.clip(pwm * self._voltage - + (self._torque_constant + self._viscous_damping) + * current_motor_velocity, + -VOLTAGE_CLIPPING, VOLTAGE_CLIPPING) + current = voltage_net / self._resistance + current_sign = np.sign(current) + current_magnitude = np.absolute(current) + + # Saturate torque based on empirical current relation. + actual_torque = np.interp(current_magnitude, self._current_table, + self._torque_table) + actual_torque = np.multiply(current_sign, actual_torque) + return actual_torque, observed_torque