def setupWorld():
p.resetSimulation()
p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
for i in range (p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
for i in range (numObjects):
cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0,0,-10)
def testJacobian(self):
import pybullet as p
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
urdfs = [
"TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf",
"kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf"
]
for urdf in urdfs:
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
robotId = p.loadURDF(urdf, useFixedBase=True)
p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
numJoints = p.getNumJoints(robotId)
endEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)])
p.stepSimulation()
# Get the joint and link state directly from Bullet.
mpos, mvel, mtorq = self.getMotorJointStates(robotId)
result = p.getLinkState(robotId,
endEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
# The localPosition is always defined in terms of the link frame coordinates.
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec,
zero_vec)
assert (allclose(dot(jac_t, mvel), link_vt))
assert (allclose(dot(jac_r, mvel), link_vr))
p.disconnect()
def evaluate_params(evaluateFunc,
params,
objectiveParams,
urdfRoot='',
timeStep=0.01,
maxNumSteps=10000,
sleepTime=0):
print('start evaluation')
beforeTime = time.time()
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("%s/plane.urdf" % urdfRoot)
p.setGravity(0, 0, -10)
global minitaur
minitaur = Minitaur(urdfRoot)
start_position = current_position()
last_position = None # for tracing line
total_energy = 0
for i in range(maxNumSteps):
torques = minitaur.getMotorTorques()
velocities = minitaur.getMotorVelocities()
total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep
joint_values = evaluate_func_map[evaluateFunc](i, params)
minitaur.applyAction(joint_values)
p.stepSimulation()
if (is_fallen()):
break
if i % 100 == 0:
sys.stdout.write('.')
sys.stdout.flush()
time.sleep(sleepTime)
print(' ')
alpha = objectiveParams[0]
final_distance = np.linalg.norm(start_position - current_position())
finalReturn = final_distance - alpha * total_energy
elapsedTime = time.time() - beforeTime
print("trial for ", params, " final_distance", final_distance, "total_energy", total_energy,
"finalReturn", finalReturn, "elapsed_time", elapsedTime)
return finalReturn
def setupWorld(self):
numObjects = 50
maximalCoordinates = False
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.loadURDF("planeMesh.urdf", useMaximalCoordinates=maximalCoordinates)
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10],
useMaximalCoordinates=maximalCoordinates)
for i in range(p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0)
for i in range(numObjects):
cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0, 0, -10)
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
self.timeStep = 0.01
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -10)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
p.resetJointState(self.cartpole, 1, initialAngle)
p.resetJointState(self.cartpole, 0, initialCartPos)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
return np.array(self.state)
def test_rolling_friction(self):
import pybullet as p
p.connect(p.DIRECT)
p.loadURDF("plane.urdf")
sphere = p.loadURDF("sphere2.urdf", [0, 0, 1])
p.resetBaseVelocity(sphere, linearVelocity=[1, 0, 0])
p.changeDynamics(sphere, -1, linearDamping=0, angularDamping=0)
#p.changeDynamics(sphere,-1,rollingFriction=0)
p.setGravity(0, 0, -10)
for i in range(1000):
p.stepSimulation()
vel = p.getBaseVelocity(sphere)
self.assertLess(vel[0][0], 1e-10)
self.assertLess(vel[0][1], 1e-10)
self.assertLess(vel[0][2], 1e-10)
self.assertLess(vel[1][0], 1e-10)
self.assertLess(vel[1][1], 1e-10)
self.assertLess(vel[1][2], 1e-10)
p.disconnect()
def _reset(self):
p.resetSimulation()
#p.setPhysicsEngineParameter(numSolverIterations=300)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"))
dist = 5 +2.*random.random()
ang = 2.*3.1415925438*random.random()
ballx = dist * math.sin(ang)
bally = dist * math.cos(ang)
ballz = 1
p.setGravity(0,0,-10)
self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def __enter__(self):
print("connecting")
optionstring='--width={} --height={}'.format(pixelWidth,pixelHeight)
optionstring += ' --window_backend=2 --render_device=0'
print(self.connection_mode, optionstring,*self.argv)
cid = pybullet.connect(self.connection_mode, options=optionstring,*self.argv)
if cid < 0:
raise ValueError
print("connected")
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,0)
pybullet.resetSimulation()
pybullet.loadURDF("plane.urdf",[0,0,-1])
pybullet.loadURDF("r2d2.urdf")
pybullet.loadURDF("duck_vhacd.urdf")
pybullet.setGravity(0,0,-10)
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
self.timeStep = 0.02
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -9.8)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
#print("randstate=",randstate)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
#print("self.state=", self.state)
return np.array(self.state)
def _reset(self):
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
xpos = 0.5 +0.2*random.random()
ypos = 0 +0.25*random.random()
ang = 3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def _reset(self):
"""Environment reset called at the beginning of an episode.
"""
# Set the camera settings.
look = [0.23, 0.2, 0.54]
distance = 1.
pitch = -56 + self._cameraRandom*np.random.uniform(-3, 3)
yaw = 245 + self._cameraRandom*np.random.uniform(-3, 3)
roll = 0
self._view_matrix = p.computeViewMatrixFromYawPitchRoll(
look, distance, yaw, pitch, roll, 2)
fov = 20. + self._cameraRandom*np.random.uniform(-2, 2)
aspect = self._width / self._height
near = 0.01
far = 10
self._proj_matrix = p.computeProjectionMatrixFOV(
fov, aspect, near, far)
self._attempted_grasp = False
self._env_step = 0
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
# Choose the objects in the bin.
urdfList = self._get_random_object(
self._numObjects, self._isTest)
self._objectUids = self._randomly_place_objects(urdfList)
self._observation = self._get_observation()
return np.array(self._observation)
def main(unused_args):
timeStep = 0.01
c = p.connect(p.SHARED_MEMORY)
if (c<0):
c = p.connect(p.GUI)
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
minitaur = Minitaur()
amplitude = 0.24795664427
speed = 0.2860877729434
for i in range(1000):
a1 = math.sin(i*speed)*amplitude+1.57
a2 = math.sin(i*speed+3.14)*amplitude+1.57
joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57]
minitaur.applyAction(joint_values)
p.stepSimulation()
# print(minitaur.getBasePosition())
time.sleep(timeStep)
final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition()))
print(final_distance)
def run():
physicsClient = p.connect(
p.GUI) # p.GUI for graphical, or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(
pybullet_data.getDataPath()) # defines the path used by p.loadURDF
p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(enableConeFriction=1)
p.setRealTimeSimulation(
0
) # only if this is set to 0 and with explicit steps will the torque control work correctly
# load the ground plane
planeId = p.loadURDF("plane.urdf")
# add a box for blocked force
boxStartPos = [0, -1, 1.4]
boxStartOr = p.getQuaternionFromEuler([0, 0, 0])
boxId, boxConstraintId = load_constrained_urdf(
"urdfs/smallSquareBox.urdf",
boxStartPos,
boxStartOr,
physicsClient=physicsClient,
)
p.changeDynamics(boxId, -1, lateralFriction=2)
# load finger
finger = SMContinuumManipulator(manipulator_definition)
finger_startPos = [0, 1, 2.05]
finger_StartOr = p.getQuaternionFromEuler([-np.pi / 2, 0, np.pi])
finger.load_to_pybullet(
baseStartPos=finger_startPos,
baseStartOrn=finger_StartOr,
baseConstraint="static",
physicsClient=physicsClient,
)
p.changeDynamics(finger.bodyUniqueId, -1, lateralFriction=2)
p.changeDynamics(finger.bodyUniqueId, -1, restitution=1)
time_step = 0.001
p.setTimeStep(time_step)
n_steps = 20000
sim_time = 0.0
lam = 1.0
omega = 1.0
torque_fns = [
lambda t: 20 * np.sin(omega * t),
lambda t: 20 * np.sin(omega * t - 1 * np.pi),
] # - 0pi goes right, - pi goes left
real_time = time.time()
normal_forces = np.zeros((n_steps, ))
normal_forces_lastLink = np.zeros((n_steps, ))
time_plot = np.zeros((n_steps, ))
last_link = p.getNumJoints(
finger.bodyUniqueId) - 1 # last link of the finger
# wait for screen recording for demo
wait_for_rec = False
if wait_for_rec:
time.sleep(5)
print("6 more seconds")
time.sleep(6)
for i in range(n_steps):
print(torque_fns[0](sim_time))
finger.apply_actuationTorque(actuator_nr=0,
axis_nr=0,
actuation_torques=torque_fns[0](sim_time))
finger.apply_actuationTorque(actuator_nr=0,
axis_nr=1,
actuation_torques=0)
p.stepSimulation()
sim_time += time_step
# time it took to simulate
delta = time.time() - real_time
real_time = time.time()
# print(delta)
contactPoints = p.getContactPoints(boxId,
finger.bodyUniqueId,
physicsClientId=physicsClient)
contactPointsTip = p.getContactPoints(
bodyA=boxId,
bodyB=finger.bodyUniqueId,
linkIndexB=last_link,
physicsClientId=physicsClient,
)
# print(" ---------- normal force ---------")
time_plot[i] = sim_time
total_normal_force = 0
for contactPoint in contactPoints:
normal_force = contactPoint[9]
total_normal_force += normal_force
total_normal_force_lastLink = 0
print(len(contactPoints), len(contactPointsTip))
for contactPointTip in contactPointsTip:
normal_forceTip = contactPointTip[9]
print(normal_forceTip)
total_normal_force_lastLink += normal_forceTip
normal_forces[i] = total_normal_force
normal_forces_lastLink[i] = total_normal_force_lastLink
p.disconnect()
plt.plot(time_plot, normal_forces)
plt.xlabel("time")
plt.ylabel("total normal force")
plt.show()
plt.plot(time_plot, normal_forces_lastLink)
plt.xlabel("time")
plt.ylabel("total normal force actuator tip")
plt.show()
import pybullet as p
p.connect(p.GUI)
useMaximalCoordinates = False
p.loadURDF("plane.urdf", useMaximalCoordinates=useMaximalCoordinates )
#p.loadURDF("sphere2.urdf",[0,0,1])
p.loadURDF("cube.urdf",[0,0,1], useMaximalCoordinates=useMaximalCoordinates )
p.setGravity(0,3,-10)
while(1):
p.stepSimulation()
pts = p.getContactPoints()
print("num pts=",len(pts))
totalNormalForce = 0
totalFrictionForce = [0,0,0]
totalLateralFrictionForce=[0,0,0]
for pt in pts:
#print("pt.normal=",pt[7])
#print("pt.normalForce=",pt[9])
totalNormalForce += pt[9]
#print("pt.lateralFrictionA=",pt[10])
#print("pt.lateralFrictionADir=",pt[11])
#print("pt.lateralFrictionB=",pt[12])
#print("pt.lateralFrictionBDir=",pt[13])
totalLateralFrictionForce[0]+=pt[11][0]*pt[10]+pt[13][0]*pt[12]
totalLateralFrictionForce[1]+=pt[11][1]*pt[10]+pt[13][1]*pt[12]
totalLateralFrictionForce[2]+=pt[11][2]*pt[10]+pt[13][2]*pt[12]
print("totalNormalForce=",totalNormalForce)
print("totalLateralFrictionForce=",totalLateralFrictionForce)
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[4] * record[11],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[5],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[5] * record[12],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[6],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[6] * record[13],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[7],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[7] * record[14],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setGravity(0.000000, 0.000000, -10.000000)
p.stepSimulation()
p.stepSimulation()
sleep(0.01)
import pybullet as p
from time import sleep
import pybullet_data
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
useDeformable = True
if useDeformable:
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
gravZ = -10
p.setGravity(0, 0, gravZ)
planeOrn = [0, 0, 0, 1] #p.getQuaternionFromEuler([0.3,0,0])
planeId = p.loadURDF("plane.urdf", [0, 0, -2], planeOrn)
boxId = p.loadURDF("cube.urdf", [0, 1, 2], useMaximalCoordinates=True)
clothId = p.loadSoftBody("bunny.obj",
basePosition=[0, 0, 2],
scale=0.5,
mass=1.,
useNeoHookean=0,
useBendingSprings=1,
useMassSpring=1,
springElasticStiffness=100,
springDampingStiffness=.001,
useSelfCollision=0,
frictionCoeff=.5,
useFaceContact=1)
import pybullet as p
import pybullet_data
client = p.connect(p.GUI)
p.setGravity(0, 0, -10, physicsClientId=client)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeID = p.loadURDF("plane.urdf")
carID = p.loadURDF("racecar/racecar.urdf", basePosition=[0, 0, 1])
# cubeID = p.loadURDF("cube_collisionfilter.urdf", [0,0,3], useMaximalCoordinates=False)
p.setCollisionFilterGroupMask(carID, -1, 0, 0)
enableCol = 1
p.setCollisionFilterPair(planeID, carID, -1, -1, enableCol)
# pos, orien = p.getBasePositionAndOrientation(carID)
for _ in range(3000):
pos, orien = p.getBasePositionAndOrientation(carID)
p.applyExternalForce(carID, 0, [40, 0, 0], pos, p.WORLD_FRAME)
p.stepSimulation()
def setup_gravity(env: AssistiveEnv):
p.setGravity(0, 0, -9.81, physicsClientId=env.id)
if not env.robot.mobile:
env.robot.set_gravity(0, 0, 0)
env.human.set_gravity(0, 0, -1)
def run_simulation_null_y_COM(dt,
t_stance,
duration,
f,
A_lat,
th0_lat,
th0_abd,
thf_abd,
z_rotation,
girdle_friction,
body_friction,
last_link_friction,
leg_friction,
K_lateral,
k0_lateral,
Kp_lat,
Kp_r_abd,
Kp_l_abd,
Kp_flex,
Kv_lat,
Kv_r_abd,
Kv_l_abd,
Kv_flex,
keep_in_check=True,
graphic_mode=False,
plot_graph_joint=False,
plot_graph_COM=False,
video_logging=False,
video_path="./video/trash/trash.mp4"):
"""
Parameters
----------
dt : float
length of simulation time-step (seconds, tipically something between 1/240 and 1/480);
values too high can cause instability in the controller due to discretization\n
t_stance : float
duration of the stance phase (50% symmetric duty cycle)\n
duration : float
total duration of the simulation\n
f : float
walking frequency, titpically should be set to 1/(2*t_stance)\n
A_lat : float
amplitude of the traveling wave used for setting the starting position\n
th0_lat : float
offset of the traveling wave used for setting the starting position\n
th0_abd : float
starting value for the leg abduction trajectory\n
thf_abd : float
ending value for the leg abduction trajectory\n
z_rotation : float
initial z-rotation of the model reference frame\n
girdle_friction : float
value of the friction (w/ the floor) of the girdle link\n
body_friction : float
value of the friction (w/ the floor) of the spinal segments\n
last_link_friction : [type]
value of the friction (w/ the floor) of the last segment of the spine\n
leg_friction : float
value of the friction (w/ the floor) of the legs link (feet's friction, substantially)\n
K_lateral : float
gain of the closed-loop inverse kinematics\n
k0_lateral : float
k0 value of the closed-loop inverse kinematics, check crawler.py for a better description\n
Kp_lat : float
spinal lateral joints' proportional gain for the computed torque control\n
Kp_r_abd : float
right abduction joint's proportional gain for the computed torque control\n
Kp_l_abd : float
left abduction joint's proportional gain for the computed torque control\n
Kp_flex : float
leg flexion joints' proportional gain for the computed torque control\n
Kv_lat : float
spinal lateral joints' derivative gain for the computed torque control\n
Kv_r_abd : float
right abduction joint's derivative gain for the computed torque control\n
Kv_l_abd : float
left abduction joint's derivative gain for the computed torque control\n
Kv_flex : float
leg flexion joints' derivative gain for the computed torque control\n
keep_in_check : bool, optional
check if the simulation have an unexpected unrealistical behaviour
(should be properly set up inside this function definition), by default True\n
graphic_mode : bool, optional
show GUI of simulation, by default False. Need to be set to True for video logging\n
plot_graph_joint : bool, optional
plot joints' parameters, by default False\n
plot_graph_COM : bool, optional
plot COM's trajectory, by default False\n
video_logging : bool, optional
save a video of the simulation, by default False\n
video_path : str, optional
filepath for the video file, by default "./video/trash/trash.mp4"\n
Returns
-------
loss: float\n
Value of the loss function of the simulation
"""
# dt, t_stance, duration = time parameter, dt MUST be the same used to create the model
# A,f1,n,t2_off,delta_off,bias = parameters for the generation of the torques
# theta_rg0, theta_lg0, A_lat_0, theta_lat_0= parameters for setting the starting position
# girdle_friction, body_friction, leg_friction = lateral friction values
##########################################
####### PYBULLET SET-UP ##################
if graphic_mode:
physicsClient = p.connect(p.GUI)
else:
physicsClient = p.connect(p.DIRECT)
p.setTimeStep(dt)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
### plane set-up
planeID = p.loadURDF("plane100.urdf", [0, 0, 0])
p.changeDynamics(
planeID,
linkIndex=-1,
lateralFriction=1,
spinningFriction=1,
rollingFriction=1,
#restitution = 0.5,
#contactStiffness = 0,
#contactDamping = 0
)
##########################################
####### MODEL SET-UP #####################
model = crw.Crawler(
urdf_path="/home/fra/Uni/Tesi/crawler/src",
dt_simulation=dt,
base_position=[0, 0, 0.05],
base_orientation=[0, 0, sin(z_rotation / 2),
cos(z_rotation / 2)],
mass_distribution=True)
# for i,elem in enumerate(model.links_state_array):
# print("Link %d "%i, elem["world_com_trn"])
# print(model.links_state_array)
# Set motion damping ("air friction")
for index in range(-1, model.num_joints):
p.changeDynamics(model.Id,
index,
linearDamping=0.0001,
angularDamping=0.0001)
# Girdle link dynamic properties
p.changeDynamics(
model.Id,
linkIndex=-1,
lateralFriction=girdle_friction,
spinningFriction=girdle_friction / 100,
rollingFriction=girdle_friction / 100,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# Body dynamic properties
for i in range(1, model.num_joints - 4, 2):
p.changeDynamics(
model.Id,
linkIndex=i,
lateralFriction=body_friction,
spinningFriction=body_friction / 100,
rollingFriction=body_friction / 100,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# # Last link dynamic properties
p.changeDynamics(
model.Id,
linkIndex=(model.control_indices[0][-1] + 1),
lateralFriction=last_link_friction,
spinningFriction=last_link_friction / 100000,
rollingFriction=last_link_friction / 100000,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# Legs dynamic properties
for i in range(model.num_joints - 3, model.num_joints, 2):
p.changeDynamics(
model.Id,
linkIndex=i,
lateralFriction=leg_friction,
spinningFriction=leg_friction / 100,
rollingFriction=leg_friction / 100,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# Dorsal joints "compliance"
model.turn_off_crawler()
for i in range(1, model.control_indices[0][-1] + 1, 2):
p.setJointMotorControl2(model.Id,
i,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=0.05)
# Starting position
model.set_bent_position(theta_rg=th0_abd,
theta_lg=-thf_abd,
A_lat=A_lat,
theta_lat_0=th0_lat)
##########################################
####### CONTROLLER PARAMETERS ############
Kp = model.generate_Kp(Kp_lat, Kp_r_abd, Kp_l_abd, Kp_flex)
Kv = model.generate_Kv(Kv_lat, Kv_r_abd, Kv_l_abd, Kv_flex)
model.set_low_pass_lateral_qa(fc=10)
model.set_low_pass_tau(fc=90)
##########################################
####### MISCELLANEOUS ####################
if graphic_mode:
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(cameraDistance=0.8,
cameraYaw=50,
cameraPitch=-60,
cameraTargetPosition=[0, 0, 0])
if (video_logging and graphic_mode):
video_writer = imageio.get_writer(video_path, fps=int(1 / dt))
##########################################
####### PERFORMANCE EVALUATION ###########
loss = 0.0
max_loss = 10000000.0 # value to set when something goes wrong, if constraints are "hard"
max_COM_height = 4 * model.body_sphere_radius
max_COM_speed = 10 #TODO: check this value
n_mean = 6
##########################################
####### TIME SERIES CREATION #############
steps = int(duration / dt)
steps_stance = int(t_stance / dt)
num_lat = len(model.control_indices[0])
f_walk = 1 / (2 * t_stance)
# Data array initialization
p_COM_time_array = np.zeros((steps, 3))
v_COM_time_array = np.zeros((steps, 3))
tau_time_array = np.zeros((steps, model.state.nqd))
joint_power_time_array = np.zeros((steps, model.state.nqd))
q_time_array = np.zeros((steps, model.state.nq))
qd_time_array = np.zeros((steps, model.state.nqd))
qdd_time_array = np.zeros((steps, model.state.nqd))
eq_time_array = np.zeros((steps, model.state.nq))
q_des_time_array = np.zeros((steps, model.state.nq))
qd_des_time_array = np.zeros((steps, model.state.nqd))
qdd_des_time_array = np.zeros((steps, model.state.nqd))
# Integrator for the work done by each joint
joint_power_integrator = crw.Integrator_Forward_Euler(
dt, np.zeros(model.state.nqd))
joint_energy_time_array = np.zeros((steps, model.state.nqd))
##########################################
####### RUN SIMULATION ###################
t = 0.0
# let the legs fall and leave the time to check the starting position
for i in range(100):
p.stepSimulation()
if graphic_mode:
time.sleep(dt)
# Initial Condition
model.state.update()
model.integrator_lateral_qa.reset(model.state.q[model.mask_act_shifted])
model.free_right_foot()
model.fix_left_foot()
model.fix_tail(second_last=False)
model.set_COM_y_ref()
qda_des_prev = np.array(([0] * len(model.control_indices[0])))
# walk and record data
for i in range(steps):
# UPDATE CONSTRAINTS
if not (i % steps_stance):
model.invert_feet()
print("step")
model.turn_off_crawler()
for tmp in range(1):
p.stepSimulation()
# UPDATE STATE
model.state.update()
p_COM_curr = np.array(model.COM_position_world())
v_COM_curr = np.array(model.COM_velocity_world())
# ADD FRAME TO VIDEO
if (graphic_mode and video_logging):
img = p.getCameraImage(800,
640,
renderer=p.ER_BULLET_HARDWARE_OPENGL)[2]
video_writer.append_data(img)
# ACTUATE MODEL AND STEP SIMULATION
qa_des, qda_des, qdda_des = model.solve_null_COM_y_speed_optimization_qdda(
qda_prev=qda_des_prev, K=K_lateral, k0=k0_lateral, filtered=True)
qda_des_prev = qda_des
q_des, qd_des, qdd_des, eq = model.generate_joints_trajectory(
theta0=th0_abd,
thetaf=thf_abd,
ti=t,
t_stance=t_stance,
qa_des=qa_des,
qda_des=qda_des,
qdda_des=qdda_des,
cos_abd=True)
tau_des, eq = model.solve_computed_torque_control(
q_des=q_des,
qd_des=qd_des,
qdd_des=qdd_des,
Kp=Kp,
Kv=Kv,
#rho=rho,
verbose=False)
tau_applied = model.apply_torques(tau_des=tau_des, filtered=True)
# UPDATE TIME-ARRAYS
if plot_graph_joint or plot_graph_COM:
q_time_array[i] = model.state.q.copy()
qd_time_array[i] = model.state.qd.copy()
qdd_time_array[i] = model.state.qdd.copy()
q_des_time_array[i] = q_des.copy()
qd_des_time_array[i] = qd_des.copy()
qdd_des_time_array[i] = qdd_des.copy()
p_COM_time_array[i] = p_COM_curr
v_COM_time_array[i] = v_COM_curr
eq_time_array[i] = q_des_time_array[i] - q_time_array[i]
tau_time_array[i] = tau_applied
for j, (v_j, tau_j) in enumerate(zip(model.state.qd, tau_applied)):
joint_power_time_array[i, j] = abs(v_j * tau_j)
joint_energy = joint_power_integrator.integrate(
joint_power_time_array[i])
joint_energy_time_array[i] = joint_energy
# print("eq: ", np.round(eq[model.mask_act],2), np.round(eq[model.mask_both_legs],2))
# print("tau: ", np.round(tau_applied[model.mask_act],2), np.round(tau_applied[model.mask_both_legs],2))
# print("qdd_des: ", np.round(qdd_des_time_array[i][model.mask_act],2), np.round(qdd_des_time_array[i][model.mask_both_legs],2))
#UPDATE LOSS - VARIATIONAL VERSION -d(x-displacement) + d(total energy expenditure)
# loss += (
# -100*(p_COM_time_array[i][0]-p_COM_time_array[i-1][0]) +
# 0.1*(joint_power_time_array[i]).dot(joint_power_time_array[i])
# )
# STEP SIMULATION
p.stepSimulation()
if graphic_mode:
time.sleep(dt)
t += dt
# CHECK SIMULATION to avoid that the model get schwifty
if keep_in_check:
# check COM height, model shouldn't fly or go through the ground
if (p_COM_curr[-1] > (max_COM_height)) or (p_COM_curr[-1] < 0.0):
print("\nLook at me I'm jumping\n")
loss += max_loss
break
if (np.linalg.norm(np.mean(v_COM_time_array[i - n_mean:i + 1], 0))
> max_COM_speed):
print("\nFaster than light. WOOOOOOOSH\n")
loss += max_loss
break
p.disconnect()
####### END SIMULATION ###################
if (video_logging and graphic_mode):
video_writer.close()
##########################################
####### PLOT DATA ########################
if plot_graph_joint:
fig_lat, axs_lat = plt.subplots(2, 3)
for i in range(0, num_lat):
#axs_lat[i//3,i%3].plot(qd_time_array[:,model.mask_act][:,i], color="xkcd:dark yellow", label="qd")
#axs_lat[i//3,i%3].plot(qdd_time_array[:,model.mask_act][:,i], color="xkcd:pale yellow", label="qdd")
#axs_lat[i//3,i%3].plot(joint_power_time_array[:,model.mask_act][:,i], color="xkcd:light salmon", label="Power")
#axs_lat[i//3,i%3].plot(joint_energy_time_array[:,model.mask_act][:,i], color="xkcd:dark salmon", label="Energy")
axs_lat[i // 3, i % 3].plot(tau_time_array[:, model.mask_act][:,
i],
color="xkcd:salmon",
label="Torque")
axs_lat[i // 3,
i % 3].plot(q_time_array[:, model.mask_act_shifted][:, i],
color="xkcd:yellow",
label="q")
axs_lat[i // 3,
i % 3].plot(q_des_time_array[:, model.mask_act_shifted][:,
i],
color="xkcd:dark teal",
label="q_des")
#axs_lat[i//3,i%3].plot(eq_time_array[:,model.mask_act_shifted][:,i], color="xkcd:red", label="error")
axs_lat[i // 3, i % 3].set_title("Lateral joint %d" % i)
handles_lat, labels_lat = axs_lat[0, 0].get_legend_handles_labels()
fig_lat.legend(handles_lat, labels_lat, loc='center right')
#
fig_girdle, axs_girdle = plt.subplots(2, 2)
for i in range(0, len(model.mask_both_legs)):
#axs_girdle[i//2,i%2].plot(qd_time_array[:,model.mask_both_legs][:,i], color="xkcd:dark yellow", label="qd")
#axs_girdle[i//2,i%2].plot(qdd_time_array[:,model.mask_both_legs][:,i], color="xkcd:pale yellow", label="qdd")
#axs_girdle[i//2,i%2].plot(joint_power_time_array[:,model.mask_both_legs][:,i], color="xkcd:light salmon", label="Power")
#axs_girdle[i//2,i%2].plot(joint_energy_time_array[:,model.mask_both_legs][:,i], color="xkcd:dark salmon", label="Energy")
axs_girdle[i // 2,
i % 2].plot(tau_time_array[:, model.mask_both_legs][:,
i],
color="xkcd:salmon",
label="Torque")
axs_girdle[i // 2, i % 2].plot(
q_time_array[:, model.mask_both_legs_shifted][:, i],
color="xkcd:yellow",
label="q")
axs_girdle[i // 2, i % 2].plot(
q_des_time_array[:, model.mask_both_legs_shifted][:, i],
color="xkcd:dark teal",
label="q_des")
axs_girdle[i // 2, i % 2].plot(
eq_time_array[:, model.mask_both_legs_shifted][:, i],
color="xkcd:red",
label="error")
axs_girdle[0, 0].set_title("Right abduction")
axs_girdle[0, 1].set_title("Right flexion")
axs_girdle[1, 0].set_title("Left abduction")
axs_girdle[1, 1].set_title("Left flexion")
handles_girdle, labels_girdle = axs_girdle[
0, 0].get_legend_handles_labels()
fig_girdle.legend(handles_girdle, labels_girdle, loc='center right')
#
plt.show()
#
if plot_graph_COM:
fig_COM = plt.figure()
axs_COM = fig_COM.add_subplot(111)
axs_COM.plot(p_COM_time_array[:, 0],
p_COM_time_array[:, 1],
color="xkcd:teal",
linewidth=4)
axs_COM.set_title("COM x-y trajectory")
axs_COM.set_xlim(-0.22, 0.01)
axs_COM.set_ylim(-0.16, 0.01)
axs_COM.set_aspect('equal')
# fig_COM_3D = plt.figure()
# axs_COM_3D = fig_COM_3D.add_subplot(111, projection='3d')
# axs_COM_3D.plot(p_COM_time_array[:,0], p_COM_time_array[:,1], p_COM_time_array[:,2],color="xkcd:teal")
# axs_COM_3D.set_title("COM 3D trajectory")
#
plt.show()
return loss
def __init__(self, config):
"""
:param config: dict
"""
if self.seed is not None:
# Set random seed from config
self.seed(config["seed"])
else:
# Set seed randomly from current time
rnd_seed = int((time.time() % 1) * 10000000)
np.random.seed(rnd_seed)
T.manual_seed(rnd_seed + 1)
self.config = config
# (x_delta, y_delta, z_delta), (qx,qy,qz,qw), (x_vel,y_vel,z_vel), (x_ang_vel, y_ang_vel, z_ang_vel)
self.raw_obs_dim = 13
# Specify what you want your observation to consist of (how many past observations of each type)
self.obs_dim = self.config["obs_input"] * self.raw_obs_dim \
+ self.config["act_input"] * 4 \
+ self.config["rew_input"] * 1 \
+ self.config["latent_input"] * 7 \
+ self.config["step_counter"] * 1
self.act_dim = 4
self.reactive_torque_dir_vec = [1, -1, -1, 1]
# Episode variables
self.step_ctr = 0
self.episode_ctr = 0
# Velocity [0,1] which the motors are currently turning at (unobservable)
self.current_motor_velocity_vec = np.array([0., 0., 0., 0.])
if (config["animate"]):
self.client_ID = p.connect(p.GUI)
else:
self.client_ID = p.connect(p.DIRECT)
# Set simulation parameters
p.setGravity(0, 0, -9.8, physicsClientId=self.client_ID)
p.setRealTimeSimulation(0, physicsClientId=self.client_ID)
p.setTimeStep(self.config["sim_timestep"],
physicsClientId=self.client_ID)
p.setAdditionalSearchPath(pybullet_data.getDataPath(),
physicsClientId=self.client_ID)
# Instantiate instances of joystick controller (human input) and pid controller
self.joystick_controller = JoyController(self.config)
self.pid_controller = PIDController(self.config)
# Load robot model and floor
self.robot, self.plane = self.load_robot()
# Define observation and action space (for gym)
self.observation_space = spaces.Box(
low=-self.config["observation_bnd"],
high=self.config["observation_bnd"],
shape=(self.obs_dim, ))
self.action_space = spaces.Box(low=-1, high=1, shape=(self.act_dim, ))
# Temporally correlated noise for disturbances
self.rnd_target_vel_source = my_utils.SimplexNoise(4, 15)
self.obs_queue = [
np.zeros(self.raw_obs_dim, dtype=np.float32) for _ in range(
np.maximum(1, self.config["obs_input"]) +
self.randomized_params["input_transport_delay"])
]
self.act_queue = [
np.zeros(self.act_dim, dtype=np.float32) for _ in range(
np.maximum(1, self.config["act_input"]) +
self.randomized_params["output_transport_delay"])
]
self.rew_queue = [
np.zeros(1, dtype=np.float32) for _ in range(
np.maximum(1, self.config["rew_input"]) +
self.randomized_params["input_transport_delay"])
]
self.create_targets()
def reset_env(self):
p.resetSimulation()
p.setPhysicsEngineParameter(enableConeFriction=1)
p.setTimeStep(self._timeStep)
# Set gravity
p.setGravity(0, 0, -9.81)
# Load plane and table
# self._planeId = p.loadURDF(self._urdfRoot+'/plane.urdf', basePosition=[0, 0, -1], useFixedBase=1)
self._tableId = p.loadURDF(self._urdfRoot + '/table/table.urdf',
basePosition=[0.400, 0.000, -0.630 + 0.005],
baseOrientation=[0., 0., 0., 1.0],
useFixedBase=1)
# Load arm, no need to settle (joint angle set instantly)
self._panda = Panda(self.long_finger, self.wide_finger)
self._pandaId = self._panda.load()
p.changeDynamics(
self._pandaId,
self._panda.pandaLeftFingerLinkIndex,
lateralFriction=self._mu,
spinningFriction=self._sigma,
frictionAnchor=1,
)
p.changeDynamics(
self._pandaId,
self._panda.pandaRightFingerLinkIndex,
lateralFriction=self._mu,
spinningFriction=self._sigma,
frictionAnchor=1,
)
p.changeDynamics(
self._tableId,
-1,
lateralFriction=self._mu,
spinningFriction=self._sigma,
frictionAnchor=1,
)
# Create a constraint to keep the fingers centered (upper links)
fingerGear = p.createConstraint(self._pandaId,
self._panda.pandaLeftFingerJointIndex,
self._pandaId,
self._panda.pandaRightFingerJointIndex,
jointType=p.JOINT_GEAR,
jointAxis=[1, 0, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(fingerGear,
gearRatio=-1,
erp=0.1,
maxForce=2 * self._panda.maxFingerForce)
# Disable damping for all links
for i in range(self._panda.numJoints):
p.changeDynamics(self._pandaId,
i,
linearDamping=0,
angularDamping=0)
def velocity(self, value):
Actuator.setVelocity(self.joint, value)
@property
def torque(self):
jointState = self.joint.getState()
return jointState.appliedTorque
@torque.setter
def torque(self, value):
Actuator.setTorque(self.joint, value)
if __name__ == "__main__":
import os
sid = pb.connect(pb.GUI)
cwd = os.getcwd()
pb.setAdditionalSearchPath(cwd + "../../data")
pb.setGravity(0, 0, -9.8, sid)
robot = BaxterRobot()
robot.resetJoints()
pb.setRealTimeSimulation(True)
for i in range(1000000):
pb.stepSimulation()
robot.controlActuators({"left_e0": 1.0}, controlType="torque")
print(robot.actuators.keys())
print(robot.joints.keys())
def __init__(self,
gui,
controlStep=0.1,
simStep=0.015,
controller=None,
jointControlMode="position",
visualizationSpeed=1.0,
boturdf=base_path + "/URDF/pexod.urdf",
floorurdf=base_path + "/URDF/plane.urdf",
lateral_friction=10.0):
# pybullet = pybullet
self.__vspeed = visualizationSpeed
self.__init_state = [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0, 0.0
] #position and orientation
self.__simStep = simStep
self.__controlStep = controlStep
self.__controller = controller
self.physicsClient = object()
assert jointControlMode in ["position", "velocity"]
self.jointControlMode = jointControlMode #either "position" or "velocity"
self.__base_collision = False
self.__heightExceed = False
self.gui = gui
self.boturdf = boturdf
self.floorurdf = floorurdf
self.lateral_friction = lateral_friction
if gui:
self.physicsClient = pybullet.connect(pybullet.GUI)
print("New GUI Client: ", self.physicsClient)
else:
self.physicsClient = pybullet.connect(pybullet.DIRECT)
print("New DIRECT Client: ", self.physicsClient)
pybullet.resetSimulation(physicsClientId=self.physicsClient)
pybullet.setTimeStep(self.__simStep,
physicsClientId=self.physicsClient)
pybullet.resetDebugVisualizerCamera(1.5,
50,
-35.0, [0, 0, 0],
physicsClientId=self.physicsClient)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath(),
physicsClientId=self.physicsClient)
pybullet.setGravity(0, 0, -10.0, physicsClientId=self.physicsClient)
self.__planeId = pybullet.loadURDF(floorurdf, [0, 0, 0],
pybullet.getQuaternionFromEuler(
[0, 0, 0]),
physicsClientId=self.physicsClient)
self.hexapodStartPos = [
0, 0, 0.2
] # Start at collision free state. Otherwise results becomes a bit random
self.hexapodStartOrientation = pybullet.getQuaternionFromEuler(
[0, 0, 0])
flags = pybullet.URDF_USE_INERTIA_FROM_FILE or pybullet.URDF_USE_SELF_COLLISION or pybullet.URDF_USE_SELF_COLLISION_INCLUDE_PARENT
self.__hexapodId = pybullet.loadURDF(
boturdf,
self.hexapodStartPos,
self.hexapodStartOrientation,
useFixedBase=0,
flags=flags,
physicsClientId=self.physicsClient)
self.joint_list = self._make_joint_list(self.__hexapodId)
self.goal_position = []
self.goalBodyID = None
self.visualGoalShapeId = pybullet.createVisualShape(
shapeType=pybullet.GEOM_CYLINDER,
radius=0.2,
length=0.04,
visualFramePosition=[0., 0., 0.],
visualFrameOrientation=pybullet.getQuaternionFromEuler([0, 0, 0]),
rgbaColor=[0.0, 0.0, 0.0, 0.5],
specularColor=[0.5, 0.5, 0.5, 1.0],
physicsClientId=self.physicsClient)
pybullet.changeDynamics(self.__planeId,
linkIndex=-1,
lateralFriction=lateral_friction,
physicsClientId=self.physicsClient)
def main():
step_per_sec = 100
num_directions = 12
obj_count = 0
root_dir = '/cvgl2/u/chengshu/ig_dataset_v5/objects'
s = Simulator(mode='headless',
image_width=512,
image_height=512,
physics_timestep=1 / float(step_per_sec))
p.setGravity(0.0, 0.0, 0.0)
for obj_class_dir in sorted(os.listdir(root_dir)):
obj_class_dir = os.path.join(root_dir, obj_class_dir)
for obj_inst_dir in os.listdir(obj_class_dir):
obj_inst_name = obj_inst_dir
urdf_path = obj_inst_name + '.urdf'
obj_inst_dir = os.path.join(obj_class_dir, obj_inst_dir)
urdf_path = os.path.join(obj_inst_dir, urdf_path)
obj = ArticulatedObject(urdf_path)
s.import_object(obj)
with open(os.path.join(obj_inst_dir, 'misc/bbox.json'),
'r') as bbox_file:
bbox_data = json.load(bbox_file)
bbox_max = np.array(bbox_data['max'])
bbox_min = np.array(bbox_data['min'])
offset = -(bbox_max + bbox_min) / 2.0
z = stable_z_on_aabb(obj.body_id, [[0, 0, 0], [0, 0, 0]])
obj.set_position([offset[0], offset[1], z])
_, extent = get_center_extent(obj.body_id)
max_half_extent = max(extent) / 2.0
px = max_half_extent * 3.0
py = 0.0
pz = extent[2] / 2.0
camera_pose = np.array([px, py, pz])
s.renderer.set_camera(camera_pose, [0, 0, pz], [0, 0, 1])
num_joints = p.getNumJoints(obj.body_id)
if num_joints == 0:
s.reload()
continue
# collect joint low/high limit
joint_low = []
joint_high = []
for j in range(num_joints):
j_low, j_high = p.getJointInfo(obj.body_id, j)[8:10]
joint_low.append(j_low)
joint_high.append(j_high)
# set joints to their lowest limits
for j, j_low in zip(range(num_joints), joint_low):
p.resetJointState(obj.body_id,
j,
targetValue=j_low,
targetVelocity=0.0)
s.sync()
# render the images
joint_low_imgs = []
for i in range(num_directions):
yaw = np.pi * 2.0 / num_directions * i
obj.set_orientation(
quatToXYZW(euler2quat(0.0, 0.0, yaw), 'wxyz'))
s.sync()
rgb, three_d = s.renderer.render(modes=('rgb', '3d'))
depth = -three_d[:, :, 2]
rgb[depth == 0] = 1.0
joint_low_imgs.append(
Image.fromarray((rgb[:, :, :3] * 255).astype(np.uint8)))
# set joints to their highest limits
for j, j_high in zip(range(num_joints), joint_high):
p.resetJointState(obj.body_id,
j,
targetValue=j_high,
targetVelocity=0.0)
s.sync()
# render the images
joint_high_imgs = []
for i in range(num_directions):
yaw = np.pi * 2.0 / num_directions * i
obj.set_orientation(
quatToXYZW(euler2quat(0.0, 0.0, yaw), 'wxyz'))
s.sync()
rgb, three_d = s.renderer.render(modes=('rgb', '3d'))
depth = -three_d[:, :, 2]
rgb[depth == 0] = 1.0
joint_high_imgs.append(
Image.fromarray((rgb[:, :, :3] * 255).astype(np.uint8)))
# concatenate the images
imgs = []
for im1, im2 in zip(joint_low_imgs, joint_high_imgs):
dst = Image.new('RGB', (im1.width + im2.width, im1.height))
dst.paste(im1, (0, 0))
dst.paste(im2, (im1.width, 0))
imgs.append(dst)
gif_path = '{}/visualizations/{}_joint_limit.gif'.format(
obj_inst_dir, obj_inst_name)
# save the gif
imgs[0].save(gif_path,
save_all=True,
append_images=imgs[1:],
optimize=True,
duration=200,
loop=0)
s.reload()
obj_count += 1
print(obj_count, gif_path)
def gravity(self, gravity: Tuple[float, float, float]): self._gravity = gravity pb.setGravity(*gravity)
if p.getJointInfo(robot, c)[3] > -1:
for r in range(3):
result[r] += jacobian[r][i] * vector[c]
i += 1
return result
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
p.loadURDF("plane.urdf",[0,0,-0.3])
kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
numJoints = p.getNumJoints(kukaId)
kukaEndEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
setJointPosition(kukaId, [0.1] * numJoints)
p.stepSimulation()
def __init__(self):
self.RENDER = True
if self.RENDER:
physicsClient = p.connect(p.GUI)
else:
physicsClient = p.connect(p.DIRECT)
PLANE_PATH = '/home/brendan/bullet3/data/'
# DATA_PATH = '/home/brendan/baselines/baselines/ddpg//'
p.setGravity(0, 0, -10)
self.planeId = p.loadURDF(PLANE_PATH + "plane.urdf")
# self.DT = 0.004
self.DT = 0.001
# self.DT = 0.01
# self.DT = 0.1
p.setTimeStep(self.DT)
self.initial_pose = [0, 0, 1]
self.pose = self.initial_pose
self.prev_pose = [self.initial_pose]
self.orien = [0, 0, 0]
self.prev_orien = [0, 0, 0]
self.initial_orientation = p.getQuaternionFromEuler([0, 0, 0])
# self.Id = p.loadURDF(DATA_PATH + "assets/mybot.urdf", self.initial_pose, self.initial_orientation, flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
self.Id = p.loadURDF("assets/mybot.urdf", self.initial_pose,
self.initial_orientation)
self.num_joints = p.getNumJoints(self.Id)
# print(self.num_joints)
self.all_joints = [
p.getJointInfo(self.Id, i) for i in range(self.num_joints)
]
# print(self.all_joints)
self.motor_names = [
"right_hip_x_joint", "right_hip_z_joint", "right_hip_y_joint",
"right_knee_joint", "right_ankle_x_joint", "right_ankle_y_joint"
]
self.motor_names += [
"left_hip_x_joint", "left_hip_z_joint", "left_hip_y_joint",
"left_knee_joint", "left_ankle_x_joint", "left_ankle_y_joint"
]
self.joints = {
name: j[0]
for j in self.all_joints for name in self.motor_names
if j[1].decode() == name
}
self.joint_numbers = [self.joints[key] for key in self.joints]
# print(self.joint_numbers)
self.left_foot_sensors = [
i[0] for i in self.all_joints if "left_foot_s" in i[1].decode()
]
self.right_foot_sensors = [
i[0] for i in self.all_joints if "right_foot_s" in i[1].decode()
]
# print(self.left_foot_sensors, self.right_foot_sensors)
high = MAX_MOTOR_POSITION * np.ones(12)
low = -high
self.action_space = spaces.Box(low, high)
self.action_size = self.action_space.shape[0]
high = np.inf * np.ones(36)
low = -high
self.observation_space = spaces.Box(low, high)
self.state_size = self.observation_space.shape[0]
self.reward_range = 10
self.spec = None
self.state_dict = {}
self.inverse_state_dict = {}
self.prev_state_dict = {}
self.prev_inverse_state_dict = {}
self.metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second': int(np.round(1.0 / self.DT))
}
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("plane.urdf",useMaximalCoordinates=True)
p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True)
gravXid = p.addUserDebugParameter("gravityX",-10,10,0)
gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
for i in range (10):
for j in range (10):
for k in range (5):
ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
while True:
gravX = p.readUserDebugParameter(gravXid)
gravY = p.readUserDebugParameter(gravYid)
gravZ = p.readUserDebugParameter(gravZid)
p.setGravity(gravX,gravY,gravZ)
time.sleep(0.01)
#kdl ok, ur_tree = kdlurdf.treeFromFile(path_to_urdf) ur_chain = ur_tree.getChain(root,tip) #rbdl urmodel = rbdl.loadModel(path_to_urdf) #u2c gantry = u2c.URDFparser() gantry.from_file(path_to_urdf) #pybullet sim = pb.connect(pb.DIRECT) pbmodel = pb.loadURDF(path_to_urdf, useFixedBase=True, flags = pb.URDF_USE_INERTIA_FROM_FILE) pb.setGravity(0, 0, -9.81) #joint info jointlist, names, q_max, q_min = gantry.get_joint_info(root, tip) n_joints = gantry.get_n_joints(root, tip) q_kdl = kdl.JntArray(n_joints) #declarations q_kdl = kdl.JntArray(n_joints) #kdl q_kdl = kdl.JntArray(n_joints) gravity_kdl = kdl.Vector() gravity_kdl[2] = -9.81 g_kdl = kdl.JntArray(n_joints) #u2c & pybullet
p.connect(p.GUI)
obUids = p.loadMJCF("mjcf/humanoid.xml")
humanoid = obUids[1]
gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
jointIds = []
paramIds = []
p.setPhysicsEngineParameter(numSolverIterations=10)
p.changeDynamics(humanoid, -1, linearDamping=0, angularDamping=0)
for j in range(p.getNumJoints(humanoid)):
p.changeDynamics(humanoid, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(humanoid, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0))
p.setRealTimeSimulation(1)
while (1):
p.setGravity(0, 0, p.readUserDebugParameter(gravId))
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(humanoid, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
time.sleep(0.01)
def __init__(self, q_init, envID, use_flat_plane, enable_pyb_GUI, dt=0.001):
# Start the client for PyBullet
if enable_pyb_GUI:
pyb.connect(pyb.GUI)
else:
pyb.connect(pyb.DIRECT)
# p.GUI for graphical version
# p.DIRECT for non-graphical version
# Load horizontal plane
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
# Either use a flat ground or a rough terrain
if use_flat_plane:
self.planeId = pyb.loadURDF("plane.urdf") # Flat plane
self.planeIdbis = pyb.loadURDF("plane.urdf") # Flat plane
pyb.resetBasePositionAndOrientation(
self.planeIdbis, [20.0, 0, 0], [0, 0, 0, 1])
else:
import random
random.seed(41)
# p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
heightPerturbationRange = 0.05
numHeightfieldRows = 256*2
numHeightfieldColumns = 256*2
heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns
height_prev = 0.0
for j in range(int(numHeightfieldColumns/2)):
for i in range(int(numHeightfieldRows/2)):
# uniform distribution
height = random.uniform(0, heightPerturbationRange)
# height = 0.25*np.sin(2*np.pi*(i-128)/46) # sinus pattern
heightfieldData[2*i+2*j *
numHeightfieldRows] = (height + height_prev) * 0.5
heightfieldData[2*i+1+2*j*numHeightfieldRows] = height
heightfieldData[2*i+(2*j+1) *
numHeightfieldRows] = (height + height_prev) * 0.5
heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows] = height
height_prev = height
# Create the collision shape based on the height field
terrainShape = pyb.createCollisionShape(shapeType=pyb.GEOM_HEIGHTFIELD, meshScale=[.05, .05, 1],
heightfieldTextureScaling=(
numHeightfieldRows-1)/2,
heightfieldData=heightfieldData,
numHeightfieldRows=numHeightfieldRows,
numHeightfieldColumns=numHeightfieldColumns)
self.planeId = pyb.createMultiBody(0, terrainShape)
pyb.resetBasePositionAndOrientation(
self.planeId, [0, 0, 0], [0, 0, 0, 1])
pyb.changeVisualShape(self.planeId, -1, rgbaColor=[1, 1, 1, 1])
if envID == 1:
# Add stairs with platform and bridge
self.stairsId = pyb.loadURDF(
"../../../../../Documents/Git-Repositories/mpc-tsid/bauzil_stairs.urdf") # ,
"""basePosition=[-1.25, 3.5, -0.1],
baseOrientation=pyb.getQuaternionFromEuler([0.0, 0.0, 3.1415]))"""
pyb.changeDynamics(self.stairsId, -1, lateralFriction=1.0)
# Create the red steps to act as small perturbations
mesh_scale = [1.0, 0.1, 0.02]
visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
fileName="cube.obj",
halfExtents=[0.5, 0.5, 0.1],
rgbaColor=[
1.0, 0.0, 0.0, 1.0],
specularColor=[0.4, .4, 0],
visualFramePosition=[
0.0, 0.0, 0.0],
meshScale=mesh_scale)
collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH,
fileName="cube.obj",
collisionFramePosition=[
0.0, 0.0, 0.0],
meshScale=mesh_scale)
for i in range(4):
tmpId = pyb.createMultiBody(baseMass=0.0,
baseInertialFramePosition=[
0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[
0.0, 0.5+0.2*i, 0.01],
useMaximalCoordinates=True)
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
tmpId = pyb.createMultiBody(baseMass=0.0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0.5, 0.5+0.2*4, 0.01],
useMaximalCoordinates=True)
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
tmpId = pyb.createMultiBody(baseMass=0.0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0.5, 0.5+0.2*5, 0.01],
useMaximalCoordinates=True)
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
# Create the green steps to act as bigger perturbations
mesh_scale = [0.2, 0.1, 0.01]
visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
fileName="cube.obj",
halfExtents=[0.5, 0.5, 0.1],
rgbaColor=[
0.0, 1.0, 0.0, 1.0],
specularColor=[0.4, .4, 0],
visualFramePosition=[
0.0, 0.0, 0.0],
meshScale=mesh_scale)
collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH,
fileName="cube.obj",
collisionFramePosition=[
0.0, 0.0, 0.0],
meshScale=mesh_scale)
for i in range(3):
tmpId = pyb.createMultiBody(baseMass=0.0,
baseInertialFramePosition=[
0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[
0.15 * (-1)**i, 0.9+0.2*i, 0.025],
useMaximalCoordinates=True)
pyb.changeDynamics(tmpId, -1, lateralFriction=1.0)
# Create sphere obstacles that will be thrown toward the quadruped
mesh_scale = [0.1, 0.1, 0.1]
visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
fileName="sphere_smooth.obj",
halfExtents=[0.5, 0.5, 0.1],
rgbaColor=[
1.0, 0.0, 0.0, 1.0],
specularColor=[0.4, .4, 0],
visualFramePosition=[
0.0, 0.0, 0.0],
meshScale=mesh_scale)
collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH,
fileName="sphere_smooth.obj",
collisionFramePosition=[
0.0, 0.0, 0.0],
meshScale=mesh_scale)
self.sphereId1 = pyb.createMultiBody(baseMass=0.4,
baseInertialFramePosition=[
0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[-0.6, 0.9, 0.1],
useMaximalCoordinates=True)
self.sphereId2 = pyb.createMultiBody(baseMass=0.4,
baseInertialFramePosition=[
0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0.6, 1.1, 0.1],
useMaximalCoordinates=True)
# Flag to launch the two spheres in the environment toward the robot
self.flag_sphere1 = True
self.flag_sphere2 = True
# Create blue spheres without collision box for debug purpose
mesh_scale = [0.015, 0.015, 0.015]
visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
fileName="sphere_smooth.obj",
halfExtents=[0.5, 0.5, 0.1],
rgbaColor=[0.0, 0.0, 1.0, 1.0],
specularColor=[0.4, .4, 0],
visualFramePosition=[
0.0, 0.0, 0.0],
meshScale=mesh_scale)
self.ftps_Ids = np.zeros((4, 5), dtype=np.int)
for i in range(4):
for j in range(5):
self.ftps_Ids[i, j] = pyb.createMultiBody(baseMass=0.0,
baseInertialFramePosition=[
0, 0, 0],
baseVisualShapeIndex=visualShapeId,
basePosition=[
0.0, 0.0, -0.1],
useMaximalCoordinates=True)
# Create green spheres without collision box for debug purpose
visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH,
fileName="sphere_smooth.obj",
halfExtents=[0.5, 0.5, 0.1],
rgbaColor=[0.0, 1.0, 0.0, 1.0],
specularColor=[0.4, .4, 0],
visualFramePosition=[
0.0, 0.0, 0.0],
meshScale=mesh_scale)
self.ftps_Ids_deb = [0] * 4
for i in range(4):
self.ftps_Ids_deb[i] = pyb.createMultiBody(baseMass=0.0,
baseInertialFramePosition=[
0, 0, 0],
baseVisualShapeIndex=visualShapeId,
basePosition=[
0.0, 0.0, -0.1],
useMaximalCoordinates=True)
"""cubeStartPos = [0.0, 0.45, 0.0]
cubeStartOrientation = pyb.getQuaternionFromEuler([0, 0, 0])
self.cubeId = pyb.loadURDF("cube_small.urdf",
cubeStartPos, cubeStartOrientation)
pyb.changeDynamics(self.cubeId, -1, mass=0.5)
print("Mass of cube:", pyb.getDynamicsInfo(self.cubeId, -1)[0])"""
# Set the gravity
pyb.setGravity(0, 0, -9.81)
# Load Quadruped robot
robotStartPos = [0, 0, 0.235+0.0045 + 0.2]
robotStartOrientation = pyb.getQuaternionFromEuler(
[0.0, 0.0, 0.0]) # -np.pi/2
pyb.setAdditionalSearchPath(
"/opt/openrobots/share/example-robot-data/robots/solo_description/robots")
self.robotId = pyb.loadURDF(
# "solo12.urdf", robotStartPos, robotStartOrientation)
"solo.urdf", robotStartPos, robotStartOrientation)
# Disable default motor control for revolute joints
#self.revoluteJointIndices = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
self.revoluteJointIndices = [0,1,3,4,6,7,9,10]
pyb.setJointMotorControlArray(self.robotId, jointIndices=self.revoluteJointIndices,
controlMode=pyb.VELOCITY_CONTROL,
targetVelocities=[
0.0 for m in self.revoluteJointIndices],
forces=[0.0 for m in self.revoluteJointIndices])
# Initialize the robot in a specific configuration
self.q_init = np.array([q_init]).transpose()
pyb.resetJointStatesMultiDof(
self.robotId, self.revoluteJointIndices, self.q_init) # q0[7:])
# Enable torque control for revolute joints
jointTorques = [0.0 for m in self.revoluteJointIndices]
pyb.setJointMotorControlArray(self.robotId, self.revoluteJointIndices,
controlMode=pyb.TORQUE_CONTROL, forces=jointTorques)
# Fix the base in the world frame
pyb.createConstraint(self.robotId, -1, -1, -1, pyb.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, robotStartPos[2]])
# Set time step for the simulation
pyb.setTimeStep(dt)
# Change camera position
pyb.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=-50, cameraPitch=-35,
cameraTargetPosition=[0.0, 0.0, 0.0])
import argparse
import os
import pybullet as p
import pybullet_data
import math
import time
p.connect(p.GUI)
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(numSolverIterations=5)
p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.)
p.setPhysicsEngineParameter(numSubSteps=1)
plane = p.loadURDF("plane.urdf")
human = p.loadMJCF("mjcf/humanoid_symmetric.xml", 0)
ob = human[0]
p.resetBasePositionAndOrientation(ob, [0, 0, 1.15], [0, 0, 0, 1])
jointPositions = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0
]
for jointIndex in range(p.getNumJoints(ob)):
p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
num = p.getNumJoints(ob)
def reset(self):
self.setup_timing()
self.task_success = 0
self.prev_target_contact_pos = np.zeros(3)
self.human, self.wheelchair, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world(
furniture_type='wheelchair',
static_human_base=True,
human_impairment='random',
print_joints=False,
gender='random')
self.robot_lower_limits = self.robot_lower_limits[
self.robot_left_arm_joint_indices]
self.robot_upper_limits = self.robot_upper_limits[
self.robot_left_arm_joint_indices]
self.reset_robot_joints()
if self.robot_type == 'jaco':
wheelchair_pos, wheelchair_orient = p.getBasePositionAndOrientation(
self.wheelchair, physicsClientId=self.id)
p.resetBasePositionAndOrientation(
self.robot,
np.array(wheelchair_pos) + np.array([-0.35, -0.3, 0.3]),
p.getQuaternionFromEuler([0, 0, -np.pi / 2.0],
physicsClientId=self.id),
physicsClientId=self.id)
joints_positions = [(3, np.deg2rad(30)), (6, np.deg2rad(-90)),
(16, np.deg2rad(-90)), (28, np.deg2rad(-90)),
(31, np.deg2rad(80)), (35, np.deg2rad(-90)),
(38, np.deg2rad(80))]
self.human_controllable_joint_indices = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
self.world_creation.setup_human_joints(
self.human,
joints_positions,
self.human_controllable_joint_indices,
use_static_joints=True,
human_reactive_force=None if self.human_control else 1,
human_reactive_gain=0.01)
p.resetBasePositionAndOrientation(
self.human, [0, 0.03, 0.89 if self.gender == 'male' else 0.86],
[0, 0, 0, 1],
physicsClientId=self.id)
human_joint_states = p.getJointStates(
self.human,
jointIndices=self.human_controllable_joint_indices,
physicsClientId=self.id)
self.target_human_joint_positions = np.array(
[x[0] for x in human_joint_states])
self.human_lower_limits = self.human_lower_limits[
self.human_controllable_joint_indices]
self.human_upper_limits = self.human_upper_limits[
self.human_controllable_joint_indices]
shoulder_pos, shoulder_orient = p.getLinkState(
self.human,
5,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
elbow_pos, elbow_orient = p.getLinkState(self.human,
7,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
wrist_pos, wrist_orient = p.getLinkState(self.human,
9,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
if self.robot_type == 'pr2':
target_pos = np.array([-0.55, 0, 0.8]) + self.np_random.uniform(
-0.05, 0.05, size=3)
target_orient = np.array(
p.getQuaternionFromEuler(np.array([0, 0, 0]),
physicsClientId=self.id))
self.position_robot_toc(
self.robot,
76, [(target_pos, target_orient)], [(shoulder_pos, None),
(elbow_pos, None),
(wrist_pos, None)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=range(29, 29 + 7),
pos_offset=np.array([0.1, 0, 0]),
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
self.world_creation.set_gripper_open_position(self.robot,
position=0.25,
left=True,
set_instantly=True)
self.tool = self.world_creation.init_tool(
self.robot,
mesh_scale=[0.001] * 3,
pos_offset=[0, 0, 0],
orient_offset=p.getQuaternionFromEuler(
[0, 0, 0], physicsClientId=self.id),
maximal=False)
elif self.robot_type == 'jaco':
target_pos = np.array([-0.5, 0, 0.8]) + self.np_random.uniform(
-0.05, 0.05, size=3)
target_orient = p.getQuaternionFromEuler(np.array(
[0, np.pi / 2.0, 0]),
physicsClientId=self.id)
self.util.ik_random_restarts(self.robot,
8,
target_pos,
target_orient,
self.world_creation,
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=[0, 1, 2, 3, 4, 5, 6],
max_iterations=1000,
max_ik_random_restarts=40,
random_restart_threshold=0.03,
step_sim=True)
self.world_creation.set_gripper_open_position(self.robot,
position=1,
left=True,
set_instantly=True)
self.tool = self.world_creation.init_tool(
self.robot,
mesh_scale=[0.001] * 3,
pos_offset=[0, 0, 0.02],
orient_offset=p.getQuaternionFromEuler(
[0, -np.pi / 2.0, 0], physicsClientId=self.id),
maximal=False)
else:
target_pos = np.array([-0.55, 0, 0.8]) + self.np_random.uniform(
-0.05, 0.05, size=3)
target_orient = p.getQuaternionFromEuler(np.array(
[0, np.pi / 2.0, 0]),
physicsClientId=self.id)
if self.robot_type == 'baxter':
self.position_robot_toc(
self.robot,
48, [(target_pos, target_orient)], [(shoulder_pos, None),
(elbow_pos, None),
(wrist_pos, None)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=range(10, 17),
pos_offset=np.array([0, 0, 0.975]),
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
else:
self.position_robot_toc(
self.robot,
19, [(target_pos, target_orient)], [(shoulder_pos, None),
(elbow_pos, None),
(wrist_pos, None)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=[0, 2, 3, 4, 5, 6, 7],
pos_offset=np.array([-0.1, 0, 0.975]),
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
self.world_creation.set_gripper_open_position(self.robot,
position=0.015,
left=True,
set_instantly=True)
self.tool = self.world_creation.init_tool(
self.robot,
mesh_scale=[0.001] * 3,
pos_offset=[0, 0.125, 0],
orient_offset=p.getQuaternionFromEuler(
[0, 0, np.pi / 2.0], physicsClientId=self.id),
maximal=False)
self.generate_target()
p.setGravity(0, 0, 0, physicsClientId=self.id)
p.setGravity(0, 0, -1, body=self.human, physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
return self._get_obs([0], [0, 0])
def clean_everything(self): #p.resetSimulation() p.setGravity(0, 0, -self.gravity) p.setDefaultContactERP(0.9) p.setPhysicsEngineParameter(fixedTimeStep=self.timestep*self.frame_skip, numSolverIterations=5, numSubSteps=self.frame_skip)
-2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193,
0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517
]
for ji in range(p.getNumJoints(ob)):
p.resetJointState(ob, ji, jointPositions[ji])
p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0)
cid0 = p.createConstraint(1, 3, 1, 6, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid0, maxForce=500.000000)
cid1 = p.createConstraint(1, 16, 1, 19, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid1, maxForce=500.000000)
cid2 = p.createConstraint(1, 9, 1, 12, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid2, maxForce=500.000000)
cid3 = p.createConstraint(1, 22, 1, 25, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid3, maxForce=500.000000)
p.setGravity(0.000000, 0.000000, 0.000000)
p.stepSimulation()
p.disconnect()
pybullet.connect(pybullet.SHARED_MEMORY)
#load URDF, given a relative or absolute file+path
obj = pybullet.loadURDF("r2d2.urdf")
posX=0
posY=3
posZ=2
obj2 = pybullet.loadURDF("kuka_lwr/kuka.urdf",posX,posY,posZ)
#query the number of joints of the object
numJoints = pybullet.getNumJoints(obj)
print (numJoints)
#set the gravity acceleration
pybullet.setGravity(0,0,-9.8)
#step the simulation for 5 seconds
t_end = time.time() + 5
while time.time() < t_end:
pybullet.stepSimulation()
print ("finished")
#remove all objects
pybullet.resetSimulation()
#disconnect from the physics server
pybullet.disconnect()
plt.ion()
img = [[1,2,3]*50]*100#np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img,interpolation='none',animated=True,label="blah")
ax = plt.gca()
pybullet.connect(pybullet.DIRECT)
#pybullet.loadPlugin("eglRendererPlugin")
pybullet.loadURDF("plane.urdf",[0,0,-1])
pybullet.loadURDF("r2d2.urdf")
pybullet.setGravity(0,0,-10)
camTargetPos = [0,0,0]
cameraUp = [0,0,1]
cameraPos = [1,1,1]
pitch = -10.0
roll=0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 200
nearPlane = 0.01
farPlane = 100
fov = 60
import pybullet as p
import pybullet_data
import os
import time
GRAVITY = -9.8
dt = 1e-3
iters = 2000
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
#p.setRealTimeSimulation(True)
p.setGravity(0, 0, GRAVITY)
p.setTimeStep(dt)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 1.13]
cubeStartOrientation = p.getQuaternionFromEuler([0., 0, 0])
botId = p.loadURDF("biped/biped2d_pybullet.urdf", cubeStartPos, cubeStartOrientation)
#disable the default velocity motors
#and set some position control with small force to emulate joint friction/return to a rest pose
jointFrictionForce = 1
for joint in range(p.getNumJoints(botId)):
p.setJointMotorControl2(botId, joint, p.POSITION_CONTROL, force=jointFrictionForce)
#for i in range(10000):
# p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
# p.stepSimulation()
#import ipdb
#ipdb.set_trace()
import time
p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000])
objects = [p.loadURDF("teddy_vhacd.urdf", -0.100000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("sphere_small.urdf", -0.100000,0.955006,1.169706,0.633232,-0.000000,-0.000000,0.773962)]
objects = [p.loadURDF("cube_small.urdf", 0.300000,0.600000,0.850000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("table_square/table_square.urdf", -1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
ob = objects[0]
jointPositions=[ 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
objects = [p.loadURDF("husky/husky.urdf", 2.000000,-5.000000,1.000000,0.000000,0.000000,0.000000,1.000000)]
ob = objects[0]
jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0.000000,0.000000,0.000000)
p.setGravity(0,0,-10)
##show this for 10 seconds
#now = time.time()
#while (time.time() < now+10):
# p.stepSimulation()
p.setRealTimeSimulation(1)
while (1):
p.setGravity(0,0,-10)
p.disconnect()
def main():
# Open GUI and set pybullet_data in the path
p.connect(p.GUI)
#p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Load plane contained in pybullet_data
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"))
# Set gravity for simulation
p.setGravity(0, 0, -9.8)
# load robot model
robotIds = p.loadSDF(
os.path.join(robot_data.getDataPath(), "iCub/icub_fixed_model.sdf"))
icubId = robotIds[0]
# set constraint between base_link and world
p.createConstraint(icubId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
[
p.getBasePositionAndOrientation(icubId)[0][0],
p.getBasePositionAndOrientation(icubId)[0][1],
p.getBasePositionAndOrientation(icubId)[0][2] * 1.2
],
p.getBasePositionAndOrientation(icubId)[1])
##init_pos for standing
# without FT_sensors
init_pos = [0] * 15 + [
-29.4, 28.8, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 44.59, 0,
0, 0
]
# with FT_sensors
#init_pos = [0]*19 + [-29.4, 28.8, 0, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 0, 44.59, 0, 0, 0]
# all set to zero
#init_pos = [0]*p.getNumJoints(icubId)
# Load other objects
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "table/table.urdf"),
[1.1, 0.0, 0.0])
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "lego/lego.urdf"),
[0.5, 0.0, 0.8])
# add debug slider
jointIds = []
paramIds = []
joints_num = p.getNumJoints(icubId)
print("len init_pos ", len(init_pos))
print("len num joints", joints_num)
for j in range(joints_num):
info = p.getJointInfo(icubId, j)
jointName = info[1]
#jointType = info[2]
jointIds.append(j)
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), info[8],
info[9], init_pos[j] / 180 * m.pi))
while True:
for i in range(joints_num):
p.setJointMotorControl2(icubId,
i,
p.POSITION_CONTROL,
targetPosition=p.readUserDebugParameter(i),
targetVelocity=0.0,
positionGain=0.25,
velocityGain=0.75,
force=50)
p.stepSimulation()
time.sleep(0.01)
import pybullet as p
import pybullet_data
from collections import namedtuple
from attrdict import AttrDict
serverMode = p.GUI # GUI/DIRECT
robotUrdfPath = "../urdf/ur5/robotiq_85_gripper_simple.urdf"
# connect to engine servers
physicsClient = p.connect(serverMode)
# p.setPhysicsEngineParameter(enableFileCaching=0)
# add search path for loadURDF
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# define world
p.setGravity(0, 0, -10) # NOTE
planeID = p.loadURDF("plane.urdf")
#######################################
### define and setup robot ###
#######################################
controlJoints = [
"robotiq_85_left_knuckle_joint", "robotiq_85_right_knuckle_joint",
"robotiq_85_left_inner_knuckle_joint",
"robotiq_85_right_inner_knuckle_joint", "robotiq_85_left_finger_tip_joint",
"robotiq_85_right_finger_tip_joint"
]
robotStartPos = [0, 0, 0.15]
robotStartOrn = p.getQuaternionFromEuler([0, 1.57, 0])
robotID = p.loadURDF(robotUrdfPath,
robotStartPos,
"--minGraphicsUpdateTimeMs=0 --mp4=\"pybullet_grasp.mp4\" --mp4fps=" +
str(fps))
else:
p = bc.BulletClient(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setPhysicsEngineParameter(maxNumCmdPer1ms=1000)
p.resetDebugVisualizerCamera(cameraDistance=1.3,
cameraYaw=38,
cameraPitch=-22,
cameraTargetPosition=[0.35, -0.13, 0])
p.setAdditionalSearchPath('assets')
p.setTimeStep(timeStep)
p.setGravity(0, -9.8, 0)
np_random, seed = seeding.np_random(1)
panda = panda_sim.FetchBulletSim(p, [0, 0, 0], np_random=np_random)
panda.control_dt = timeStep
# logId = panda.bullet_client.startStateLogging(panda.bullet_client.STATE_LOGGING_PROFILE_TIMINGS, "log.json")
# panda.bullet_client.submitProfileTiming("start")
for i in range(100000):
# panda.bullet_client.submitProfileTiming("full_step")
panda.step(target=np.array([
-0.1014052646308704954, 0.22013282199293425, -0.4641104737542781, 0.01
]),
rendering=True,
time_step=timeStep)
img = np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img, interpolation='none', animated=True, label="blah")
ax = plt.gca()
#pybullet.connect(pybullet.GUI)
pybullet.connect(pybullet.DIRECT)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadURDF("plane.urdf", [0, 0, -1])
pybullet.loadURDF("r2d2.urdf")
camTargetPos = [0, 0, 0]
cameraUp = [0, 0, 1]
cameraPos = [1, 1, 1]
pybullet.setGravity(0, 0, -10)
pitch = -10.0
roll = 0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 200
nearPlane = 0.01
farPlane = 100
fov = 60
main_start = time.time()
while (1):
def initPhysicsClient():
physicsClient = pybullet.connect(pybullet.GUI)
pybullet.setGravity(0, 0, -9.81)
pybullet.setPhysicsEngineParameter(fixedTimeStep=dt, numSubSteps=1)
return physicsClient
import pybullet as p
import time
p.connect(p.GUI)
offset = [0, 0, 0]
turtle = p.loadURDF("turtlebot.urdf", offset)
plane = p.loadURDF("plane.urdf")
p.setRealTimeSimulation(1)
for j in range(p.getNumJoints(turtle)):
print(p.getJointInfo(turtle, j))
forward = 0
turn = 0
while (1):
p.setGravity(0, 0, -10)
time.sleep(1. / 240.)
keys = p.getKeyboardEvents()
leftWheelVelocity = 0
rightWheelVelocity = 0
speed = 10
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = -0.5
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = 0.5
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
turn = 0
def __init__(self,
use_interactive=True,
is_interactive_realtime=True,
hide_ui=True,
topdown_viewport=False,
log_dir=None,
log_bullet_states=False,
log_mp4=False,
robot_skew=0.0):
"""Sets up simulation elements.
Two sets of preferences affect how the simulation behaves. Choosing a
interactive simulation session allows you to interact with the robot
through keyboard and mouse. Headless sessions run much quicker and
can be used for automated testing.
Logging helps narrow down how and when the robot fails. A mp4 video
from the viewport and/or .bullet restorable states can be saved to
the log folder.
:param use_interactive: determines whether the pybullet simulation
runs with a GUI open or headless.
:param is_interactive_realtime: when interactive, choose manual timestepping
or run realtime. headless sim only uses
manual timestepping.
:param hide_ui: when interactive, choose to hide UI elements.
:param topdown_viewport: when interactive, choose between default
and topdown viewports.
:param log_dir: the directory in which to log.
:param log_bullet_states: logs .bullet sim states every 5.0 sec or
every 1200 iterations.
:param log_mp4: when interactive, logs .mp4 video until sim
completes via a graceful stop (corrupts log
if sim ends ungracefully).
:param robot_skew: [-inf, inf] where negative values skew left,
0 is no skew, and positive skew right
"""
self.use_interactive = use_interactive
self.is_interactive_realtime = is_interactive_realtime
self.hide_ui = hide_ui
self.topdown_viewport = topdown_viewport
self.log_dir = log_dir
self.log_bullet_states = log_bullet_states
self.log_mp4 = log_mp4
self.TIMESTEPPING_DT = 1 / 240
self.PRESSED_THRES = -.0038
p.connect(p.GUI if self.use_interactive else p.DIRECT)
p.resetSimulation()
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0 if self.hide_ui else 1)
if self.topdown_viewport:
p.resetDebugVisualizerCamera(1.1, 0.0, -89.9, (0.0, 0.0, 0.0))
else:
p.resetDebugVisualizerCamera(2, 30.0, -50.0, (0.0, 0.2, 0.0))
p.setGravity(0, 0, -9.8)
if self.log_mp4:
p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4,
os.join(self.log_dir, "log.mp4"))
p.setRealTimeSimulation(1 if self.is_interactive_realtime else 0)
self.mobile_agent = BlockStackerAgent(skew=robot_skew)
self.field = Field()
self.legos = Legos()
linkIndexA=0,
linkIndexB=0,
enableCollision=0)
p.setCollisionFilterPair(bodyUniqueIdA=pendulum_uniqueId_pybullet,
bodyUniqueIdB=pendulum_uniqueId_z3,
linkIndexA=0,
linkIndexB=-1,
enableCollision=0)
p.setCollisionFilterPair(bodyUniqueIdA=pendulum_uniqueId_pybullet,
bodyUniqueIdB=pendulum_uniqueId_z3,
linkIndexA=-1,
linkIndexB=0,
enableCollision=0)
# enables gravity
p.setGravity(0, 0, -9.8)
# sets real time simulation
p.setRealTimeSimulation(
enableRealTimeSimulation=0
) # now we dont have to call p.stepSimulation() in order to advance the time step of the simulation environment
# turn off all motors so that joints are not stiffened for the rest of the simulations
p.setJointMotorControlArray(bodyUniqueId=pendulum_uniqueId_pybullet,
jointIndices=list(range(number_of_links_urdf)),
controlMode=p.TORQUE_CONTROL,
positionGains=[0.1] * number_of_links_urdf,
velocityGains=[0.1] * number_of_links_urdf,
forces=[0] * number_of_links_urdf)
# load the block thatwe are trying to avoid
import pybullet as p
import cv2
import pybullet_data
import os
import time
import math
import numpy as np
file_name = "humanoid.urdf"
file2 = "humanoid/humanoid.urdf"
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf", 0, 0, 0)
robot = p.loadURDF(file_name, [0, 0, 1])
# husky = p.loadURDF("husky/husky.urdf", 0, 0, 0.1)
# quat=p.getQuaternionFromEuler([1.57,0,0])
# p.resetBasePositionAndOrientation(robot, [0, 0, 5],quat)
# cap=cv2.VideoCapture(0)
p.setGravity(0, 0, 0)
print(p.getNumJoints(robot))
# p.createConstraint(robot,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1])
while True:
# p.setJointMotorControl2(robot,22,p.VELOCITY_CONTROL,-0.00001,5)
p.stepSimulation()
time.sleep(1. / 240.0)
# p.stepSimulation()
min_dist = 3 #min disrance the balls can be from each other
k_vel = 3 #constant that varies the velocity
rolling_mass = 5 # mass of the rolling ball
static_mass = 10 # mass of the initially static ball
restitution = 0.97 #bouncyness of contact. Keep it a bit less than 1, preferably closer to 0.
gravity = -9.8
#rollingFriction : torsional friction orthogonal to contact normal (keep this value very close to zero,
#otherwise the simulation can become very unrealistic.)
rollingFriction = 0.0
random.seed()
physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0, 0, gravity)
planeId = p.loadURDF("plane.urdf")
perlin_id = p.loadTexture("perlin.png")
white_id = p.loadTexture("white.png")
#Get random distance from the center ball
while (True):
r_dist = random.randint(-max_dist, max_dist)
if abs(r_dist) >= min_dist: #have a minimum distance
break
#get random angle
angle = math.radians(random.randint(0, 360))
x_dist = r_dist * math.cos(angle)
y_dist = r_dist * math.sin(angle)
import pybullet as p
import time
import pkgutil
egl = pkgutil.get_loader('eglRenderer')
p.connect(p.DIRECT)
plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
print("plugin=",plugin)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setGravity(0,0,-10)
p.loadURDF("plane.urdf",[0,0,-1])
p.loadURDF("r2d2.urdf")
pixelWidth = 320
pixelHeight = 220
camTargetPos = [0,0,0]
camDistance = 4
pitch = -10.0
roll=0
upAxisIndex = 2
while (p.isConnected()):
for yaw in range (0,360,10) :
start = time.time()
p.stepSimulation()
def generate_world(self,
agent='r2d2.urdf',
escapeLength=50,
corridorLength=5,
numObstacles=10,
obstacleOpeningLength=1,
r2d2DistanceAheadOfWall=3,
seed=42):
totalLength = escapeLength + r2d2DistanceAheadOfWall
np.random.seed(seed)
distanceBetweenObstacles = escapeLength / numObstacles
p.resetSimulation()
p.setGravity(0, 0, -9.8)
self.planeId = p.loadURDF("plane.urdf")
self.r2d2Id = p.loadURDF(agent, self.spawnPos, self.spawnOrn)
# Create walls enclosing tunnel
self.nsHalfExtents = [
corridorLength, self.wallThickness, self.wallHeight
]
self.ewHalfExtents = [
self.wallThickness, totalLength / 2, self.wallHeight
]
self.nWallCollisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX, halfExtents=self.nsHalfExtents)
self.sWallCollisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX, halfExtents=self.nsHalfExtents)
self.eWallCollisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX, halfExtents=self.ewHalfExtents)
self.wWallCollisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX, halfExtents=self.ewHalfExtents)
self.nWallVisualShapeId = p.createVisualShape(
shapeType=p.GEOM_BOX,
rgbaColor=self.wallColor,
halfExtents=self.nsHalfExtents)
self.sWallVisualShapeId = p.createVisualShape(
shapeType=p.GEOM_BOX,
rgbaColor=self.wallColor,
halfExtents=self.nsHalfExtents)
self.eWallVisualShapeId = p.createVisualShape(
shapeType=p.GEOM_BOX,
rgbaColor=self.wallColor,
halfExtents=self.ewHalfExtents)
self.wWallVisualShapeId = p.createVisualShape(
shapeType=p.GEOM_BOX,
rgbaColor=self.wallColor,
halfExtents=self.ewHalfExtents)
self.nWallId = p.createMultiBody(
basePosition=[0, escapeLength, self.wallHeight],
baseCollisionShapeIndex=self.nWallCollisionShapeId,
baseVisualShapeIndex=self.nWallVisualShapeId)
self.sWallId = p.createMultiBody(
basePosition=[0, -r2d2DistanceAheadOfWall, self.wallHeight],
baseCollisionShapeIndex=self.sWallCollisionShapeId,
baseVisualShapeIndex=self.sWallVisualShapeId)
self.eWallId = p.createMultiBody(
basePosition=[
corridorLength,
-r2d2DistanceAheadOfWall / 2 + escapeLength / 2,
self.wallHeight
],
baseCollisionShapeIndex=self.eWallCollisionShapeId,
baseVisualShapeIndex=self.eWallVisualShapeId)
self.wWallId = p.createMultiBody(
basePosition=[
-corridorLength,
-r2d2DistanceAheadOfWall / 2 + escapeLength / 2,
self.wallHeight
],
baseCollisionShapeIndex=self.wWallCollisionShapeId,
baseVisualShapeIndex=self.wWallVisualShapeId)
self.walls.extend(
[self.nWallId, self.sWallId, self.eWallId, self.wWallId])
# Generate obstacles
for i in range(numObstacles):
obstacleYCord = distanceBetweenObstacles * (
i) + r2d2DistanceAheadOfWall
max_length = corridorLength - obstacleOpeningLength
westX = np.random.rand() * max_length
eastX = corridorLength - westX - obstacleOpeningLength
westWallCollisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX,
halfExtents=[westX, self.wallThickness, self.wallHeight])
westWallVisualShapeId = p.createVisualShape(
shapeType=p.GEOM_BOX,
rgbaColor=self.wallColor,
halfExtents=[westX, self.wallThickness, self.wallHeight])
eastWallCollisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX,
halfExtents=[eastX, self.wallThickness, self.wallHeight])
eastWallVisualShapeId = p.createVisualShape(
shapeType=p.GEOM_BOX,
rgbaColor=self.wallColor,
halfExtents=[eastX, self.wallThickness, self.wallHeight])
eastWallBasePosition = [
corridorLength - self.wallThickness - eastX, obstacleYCord,
self.wallHeight
]
westWallBasePosition = [
-corridorLength + westX, obstacleYCord, self.wallHeight
]
wObstacleId = p.createMultiBody(
baseCollisionShapeIndex=eastWallCollisionShapeId,
baseVisualShapeIndex=eastWallVisualShapeId,
basePosition=eastWallBasePosition)
eObstacleId = p.createMultiBody(
baseCollisionShapeIndex=westWallCollisionShapeId,
baseVisualShapeIndex=westWallVisualShapeId,
basePosition=westWallBasePosition)
self.walls.extend([wObstacleId, eObstacleId])
import pybullet as p
from time import sleep
import matplotlib.pyplot as plt
import numpy as np
physicsClient = p.connect(p.GUI)
p.setGravity(0,0,0)
bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
camera = p.getDebugVisualizerCamera()
viewMat = camera[2]
projMat = camera[3]
#An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
from seagul.resources import getSgResourcePath
import pybullet_data
from numpy import pi
import math
import os
import time
GRAVITY = -9.8
dt = 1e-3
iters = 2000
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
# p.setRealTimeSimulation(True)
p.setGravity(0, 0, GRAVITY)
p.setTimeStep(dt)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 1.13]
cubeStartOrientation = p.getQuaternionFromEuler([0.0, 0, 0])
p.setAdditionalSearchPath(getSgResourcePath())
botId = p.loadURDF("five_link.urdf", cubeStartPos, cubeStartOrientation)
init_pos = [-0.86647779, -5.57969548, 4.56618282, -0.86647779]
init_vel = [-0.08985754, 2.59193943, -0.48066481, 1.88797459]
for i in range(len(init_pos)):
p.resetJointState(botId, i + 1, init_pos[i], init_vel[i])
p.resetBasePositionAndOrientation(botId, [0, 0, 0],
p.getQuaternionFromEuler([0, pi, 0]))
import pybullet as p
p.connect(p.GUI)
cube = p.loadURDF("cube.urdf")
frequency = 240
timeStep = 1. / frequency
p.setGravity(0, 0, -9.8)
p.changeDynamics(cube, -1, linearDamping=0, angularDamping=0)
p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
for i in range(frequency):
p.stepSimulation()
pos, orn = p.getBasePositionAndOrientation(cube)
print(pos)
def reset(self):
self.terminated = False
self.n_contacts = 0
self.n_steps_outside = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timestep)
p.loadURDF(os.path.join(self._urdf_root, "plane.urdf"), [0, 0, -1])
self.table_uid = p.loadURDF(
os.path.join(self._urdf_root, "table/table.urdf"), 0.5000000,
0.00000, -.820000, 0.000000, 0.000000, 0.0, 1.0)
# Initialize button position
x_pos = 0.5
y_pos = 0
if self._random_target:
x_pos += 0.15 * self.np_random.uniform(-1, 1)
y_pos += 0.3 * self.np_random.uniform(-1, 1)
self.button_uid = p.loadURDF("/urdf/simple_button.urdf",
[x_pos, y_pos, Z_TABLE])
self.button_pos = np.array([x_pos, y_pos, Z_TABLE])
p.setGravity(0, 0, -10)
self._kuka = kuka.Kuka(urdf_root_path=self._urdf_root,
timestep=self._timestep,
use_inverse_kinematics=(not self.action_joints),
small_constraints=(not self._random_target))
self._env_step_counter = 0
# Close the gripper and wait for the arm to be in rest position
for _ in range(500):
if self.action_joints:
self._kuka.applyAction(
list(np.array(self._kuka.joint_positions)[:7]) + [0, 0])
else:
self._kuka.applyAction([0, 0, 0, 0, 0])
p.stepSimulation()
# Randomize init arm pos: take 5 random actions
for _ in range(N_RANDOM_ACTIONS_AT_INIT):
if self._is_discrete:
action = [0, 0, 0, 0, 0]
sign = 1 if self.np_random.rand() > 0.5 else -1
action_idx = self.np_random.randint(3) # dx, dy or dz
action[action_idx] += sign * DELTA_V
else:
if self.action_joints:
joints = np.array(self._kuka.joint_positions)[:7]
joints += DELTA_THETA * self.np_random.normal(joints.shape)
action = list(joints) + [0, 0]
else:
action = np.zeros(5)
rand_direction = self.np_random.normal((3, ))
# L2 normalize, so that the random direction is not too high or too low
rand_direction /= np.linalg.norm(rand_direction, 2)
action[:3] += DELTA_V_CONTINUOUS * rand_direction
self._kuka.applyAction(list(action))
p.stepSimulation()
self._observation = self.getExtendedObservation()
self.button_pos = np.array(
p.getLinkState(self.button_uid, BUTTON_LINK_IDX)[0])
self.button_pos[
2] += BUTTON_DISTANCE_HEIGHT # Set the target position on the top of the button
if self.saver is not None:
self.saver.reset(self._observation, self.getTargetPos(),
self.getGroundTruth())
if self.srl_model != "raw_pixels":
return self.getSRLState(self._observation)
return np.array(self._observation)
