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()
"""This file models a spot using pybullet. """ 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",
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]
sys.path.append('../../../') from spotmicro.util import pybullet_data as pd physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally p.setGravity(0, 0, -9.81) # p.setTimeStep(1./240.) # slow, accurate p.setRealTimeSimulation(0) # we want to be faster than real time :) planeId = p.loadURDF("plane.urdf") StartPos = [0, 0, 0.3] StartOrientation = p.getQuaternionFromEuler([0, 0, 0]) p.resetDebugVisualizerCamera(cameraDistance=0.8, cameraYaw=45, cameraPitch=-30, cameraTargetPosition=[0, 0, 0]) boxId = p.loadURDF(pd.getDataPath() + "/assets/urdf/spot.urdf", StartPos, StartOrientation, flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT) numj = p.getNumJoints(boxId) numb = p.getNumBodies() Pos, Orn = p.getBasePositionAndOrientation(boxId) print(Pos, Orn) print("Number of joints {}".format(numj)) print("Number of links {}".format(numb)) joint = [] movingJoints = [ 6, 7, 8, # FL