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
