def get_cube(x, y, z):
body = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cube_small.urdf"), x, y, z)
p.changeDynamics(body,-1, mass=1.2)#match Roboschool
part_name, _ = p.getBodyInfo(body, 0)
part_name = part_name.decode("utf8")
bodies = [body]
return BodyPart(part_name, bodies, 0, -1)
def episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() if (self.stadiumLoaded==0): self.stadiumLoaded=1 # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") self.ground_plane_mjcf = p.loadSDF(filename) for i in self.ground_plane_mjcf: p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) for j in range(p.getNumJoints(i)): p.changeDynamics(i,j,lateralFriction=0)
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 episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() if (self.stadiumLoaded==0): self.stadiumLoaded=1 # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"plane_stadium.sdf") self.ground_plane_mjcf=p.loadSDF(filename) #filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") #self.ground_plane_mjcf = p.loadSDF(filename) # for i in self.ground_plane_mjcf: p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) p.changeVisualShape(i,-1,rgbaColor=[1,1,1,0.8]) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
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)
import pybullet as p
import time
p.connect(p.GUI)
cube2 = p.loadURDF("cube.urdf", [0, 0, 3], useFixedBase=True)
cube = p.loadURDF("cube.urdf", useFixedBase=True)
p.setGravity(0, 0, -10)
timeStep = 1. / 240.
p.setTimeStep(timeStep)
p.changeDynamics(cube2, -1, mass=1)
#now cube2 will have a floating base and move
while (p.isConnected()):
p.stepSimulation()
time.sleep(timeStep)
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
halfpi = 1.57079632679
twopi = 4 * halfpi
kneeangle = -2.1834
dyn = p.getDynamicsInfo(quadruped, -1)
mass = dyn[0]
friction = dyn[1]
localInertiaDiagonal = dyn[2]
print("localInertiaDiagonal", localInertiaDiagonal)
#this is a no-op, just to show the API
p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal)
#for i in range (nJoints):
# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
drawInertiaBox(quadruped, -1, [1, 0, 0])
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
for i in range(nJoints):
drawInertiaBox(quadruped, i, [0, 1, 0])
if (useMaximalCoordinates):
steps = 400
for aa in range(steps):
p.setJointMotorControl2(quadruped, motor_front_leftL_joint, p.POSITION_CONTROL,
motordir[0] * halfpi * float(aa) / steps)
tableBasePosition,
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(sphereUid,
-1,
spinningFriction=0.001,
rollingFriction=0.001,
linearDamping=0.0)
for joint in range(p.getNumJoints(sphereUid)):
p.setJointMotorControl2(sphereUid,
joint,
p.VELOCITY_CONTROL,
targetVelocity=1,
force=10)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
p.getNumJoints(sphereUid)
for i in range(p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid, i)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
planeId = p.loadURDF("plane.urdf")
# load robot
startPos = [0, 0, 1]
startPos2 = [0, 0, 1.1]
startOrientation = p.getQuaternionFromEuler([0, 0, 0])
bid2 = p.loadURDF("TwoJointRobot.urdf",
startPos2,
startOrientation,
useFixedBase=True)
bid = p.loadURDF("TwoJointRobot.urdf",
startPos,
startOrientation,
useFixedBase=True)
#p.changeDynamics(bid,-1,linearDamping=0, angularDamping=0)
p.changeDynamics(bid, -1, maxJointVelocity=1000)
simt = 0
ph = 0
tLastPrint = 0
while simt < 0.5:
if simt < 0.2:
freq = 50 # Hz
elif simt < 0.4:
freq = 100
elif simt < 0.6:
freq = 200
elif simt < 0.8:
freq = 400
else:
planeID = p.loadURDF("plane100.urdf", [0, 0, 0])
z_rotation = pi / 4 #radians
model = crw.Crawler(
urdf_path="/home/fra/Uni/Tesi/crawler",
dt_simulation=dt,
base_position=[0, 0, 0.005],
base_orientation=[0, 0, sin(z_rotation / 2),
cos(z_rotation / 2)],
mass_distribution=True)
np.random.seed()
### plane set-up
p.changeDynamics(
planeID,
linkIndex=-1,
lateralFriction=1,
spinningFriction=1,
rollingFriction=1,
#restitution = 0.5,
#contactStiffness = 0,
#contactDamping = 0
)
##########################################
####### MODEL SET-UP #####################
##########################################
# Remove motion damping ("air friction")
for index in range(-1, model.num_joints):
p.changeDynamics(model.Id,
index,
linearDamping=0.00001,
angularDamping=0.00001)
# Girdle link dynamic properties
import pybullet as p
import time
import math
p.connect(p.GUI)
planeId = p.loadURDF(fileName="plane.urdf", baseOrientation=[0.25882, 0, 0, 0.96593])
p.loadURDF(fileName="cube.urdf", basePosition=[0, 0, 2])
cubeId = p.loadURDF(fileName="cube.urdf", baseOrientation=[0, 0, 0, 1], basePosition=[0, 0, 4])
#p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=0.1)
p.changeDynamics(bodyUniqueId=2, linkIndex=-1, mass=1.0)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
def drawInertiaBox(parentUid, parentLinkIndex):
mass, frictionCoeff, inertia = p.getDynamicsInfo(bodyUniqueId=parentUid,
linkIndex=parentLinkIndex,
flags=p.DYNAMICS_INFO_REPORT_INERTIA)
Ixx = inertia[0]
Iyy = inertia[1]
Izz = inertia[2]
boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
[-halfExtents[0], halfExtents[1], halfExtents[2]],
[halfExtents[0], -halfExtents[1], halfExtents[2]],
[-halfExtents[0], -halfExtents[1], halfExtents[2]],
[halfExtents[0], halfExtents[1], -halfExtents[2]],
import pybullet as p
import pybullet_data
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadSDF("stadium.sdf")
p.setGravity(0, 0, -10)
objects = p.loadMJCF("mjcf/sphere.xml")
sphere = objects[0]
p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1])
p.changeDynamics(sphere, -1, linearDamping=0.9)
p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
forward = 0
turn = 0
forwardVec = [2, 0, 0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5]
keys = p.getKeyboardEvents()
flags = p.URDF_ENABLE_SLEEPING
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.loadURDF("plane100.urdf",flags=flags, useMaximalCoordinates=useMaximalCoordinates)
#p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags)
r2d2 = -1
for k in range (5):
for i in range (5):
r2d2=p.loadURDF("r2d2.urdf",[k*2,i*2,1], useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES+flags)
#enable sleeping: you can pass the flag during URDF loading, or do it afterwards
#p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
for j in range (p.getNumJoints(r2d2)):
p.setJointMotorControl2(r2d2,j,p.VELOCITY_CONTROL,targetVelocity=0)
print("r2d2=",r2d2)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
timestep = 1./240.
p.setTimeStep(timestep)
p.setGravity(0,0,-10)
while p.isConnected():
p.stepSimulation()
time.sleep(timestep)
#force the object to wake up
p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_WAKE_UP)
def __init__(self,
robot,
model_id,
gravity,
timestep,
frame_skip,
env=None):
Scene.__init__(self, gravity, timestep, frame_skip, env)
# contains cpp_world.clean_everything()
# stadium_pose = cpp_household.Pose()
# if self.zero_at_running_strip_start_line:
# stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants
filename = os.path.join(get_model_path(model_id), "mesh_z_up.obj")
#filename = os.path.join(get_model_path(model_id), "3d", "blender.obj")
#textureID = p.loadTexture(os.path.join(get_model_path(model_id), "3d", "rgb.mtl"))
if robot.model_type == "MJCF":
MJCF_SCALING = robot.mjcf_scaling
scaling = [
1.0 / MJCF_SCALING, 1.0 / MJCF_SCALING, 0.6 / MJCF_SCALING
]
else:
scaling = [1, 1, 1]
magnified = [2, 2, 2]
collisionId = p.createCollisionShape(
p.GEOM_MESH,
fileName=filename,
meshScale=scaling,
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
view_only_mesh = os.path.join(get_model_path(model_id),
"mesh_view_only_z_up.obj")
if os.path.exists(view_only_mesh):
visualId = p.createVisualShape(p.GEOM_MESH,
fileName=view_only_mesh,
meshScale=scaling,
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
else:
visualId = -1
boundaryUid = p.createMultiBody(baseCollisionShapeIndex=collisionId,
baseVisualShapeIndex=visualId)
p.changeDynamics(boundaryUid, -1, lateralFriction=1)
#print(p.getDynamicsInfo(boundaryUid, -1))
self.scene_obj_list = [(boundaryUid, -1)] # baselink index -1
planeName = os.path.join(pybullet_data.getDataPath(),
"mjcf/ground_plane.xml")
self.ground_plane_mjcf = p.loadMJCF(planeName)
if "z_offset" in self.env.config:
z_offset = self.env.config["z_offset"]
else:
z_offset = -10 #with hole filling, we don't need ground plane to be the same height as actual floors
p.resetBasePositionAndOrientation(self.ground_plane_mjcf[0],
posObj=[0, 0, z_offset],
ornObj=[0, 0, 0, 1])
p.changeVisualShape(
boundaryUid,
-1,
rgbaColor=[168 / 255.0, 164 / 255.0, 92 / 255.0, 1.0],
specularColor=[0.5, 0.5, 0.5])
#p.resetBasePositionAndOrientation(body_id,)
colBoxId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[5, 5, 5])
ground_ID = p.createMultiBody(0, colBoxId, -1, [0, 0, -5], [0, 0, 0, 1])
#p.setGravity(0.0,0.0,-9.81)
m = Man(physicsClient, self_collisions=True)
m.resetGlobalTransformation([0, -3, 0.94], [0, 0, 0])
bodies = [ground_ID, m.body_id, body_id]
for body in bodies:
for i in range(-1, p.getNumJoints(body)):
p.changeDynamics(body,
i,
restitution=0.0,
lateralFriction=0.0,
rollingFriction=0.0,
spinningFriction=0.0)
#m_ref = Man(physicsClient, self_collisions = True)
#m_ref.resetGlobalTransformation([2,0,0.94],[0,0,0])
#p.resetBaseVelocity(body_id, [0,-1.0*2.4,0],[0,0,0])
p.resetBaseVelocity(body_id, [0, -1.0, 0], [0, 0, 0])
time.sleep(3)
hit = False
while not hit:
m.advance()
p.stepSimulation()
jr = np.array(ll) - np.array(ul)
ik_solver = p.IK_DLS
# configure arm
for i in range(num_joints):
p.resetJointState(arm_id, i, rp[i])
p.enableJointForceTorqueSensor(arm_id, i)
# extract joint names
joint_id = {}
for i in range(p.getNumJoints(arm_id)):
jointInfo = p.getJointInfo(arm_id, i)
joint_id[jointInfo[1].decode('UTF-8')] = jointInfo[0]
# adjust block mass
weight_joint_id = joint_id["weight_joint"]
p.changeDynamics(arm_id, weight_joint_id, mass=0.5)
# set up variables needed during sim
len_seconds = 5
# number of steps to wait before recording torques
burn_in = 150
start_time = time.time()
controlled_joints = list(range(eef_index - 1))
num_controlled_joints = eef_index - 1
orn = p.getQuaternionFromEuler([math.pi / 2, math.pi / 2, math.pi / 2])
targetVelocities = [0] * num_controlled_joints
forces = [500] * num_controlled_joints
positionGains = [0.03] * num_controlled_joints
velocityGains = [1] * num_controlled_joints
# read pvt file
file_name = './pvt_data/translation.csv'
read_csv = read_csv.ReadCsv(file_name)
pvt = read_csv()
# set env.
physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
snakeStartPos = [0, 0, 0.5]
snakeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
snakeId = p.loadURDF("./urdf/trident_snake.urdf", snakeStartPos,
snakeStartOrientation)
p.changeDynamics(bodyUniqueId=snakeId, linkIndex=2, lateralFriction=1)
p.changeDynamics(bodyUniqueId=snakeId, linkIndex=5, lateralFriction=1)
p.changeDynamics(bodyUniqueId=snakeId, linkIndex=7, lateralFriction=1)
timestep = 0
# enable joint F/T sensor
num_joints = p.getNumJoints(snakeId)
for i in range(num_joints):
p.enableJointForceTorqueSensor(snakeId, i)
def get_joint_data_array(body_id, joint_id):
joint_ft = np.array(p.getJointState(body_id, joint_id)[2])
link_pos_w = np.array(p.getLinkState(body_id, joint_id)[0])
link_orn_w = np.array(
p.getEulerFromQuaternion(list(p.getLinkState(body_id, joint_id)[1])))
colors=np.array([[0, 0, 1]]),
radii=blue_radius)
blue_ball_coll = p.createCollisionShape(
p.GEOM_SPHERE,
radius=blue_radius)
blue_ball = p.createMultiBody(baseMass=0.5,
baseCollisionShapeIndex=blue_ball_coll,
basePosition=[-10, 0, 0],
baseOrientation=[0, 0, 0, 1])
###############################################################################
# We set the coefficient of restitution of both the balls to `0.6`.
p.changeDynamics(red_ball, -1, restitution=0.6)
p.changeDynamics(blue_ball, -1, restitution=0.6)
###############################################################################
# We add all the actors to the scene.
scene = window.Scene()
scene.add(actor.axes())
scene.add(red_ball_actor)
scene.add(blue_ball_actor)
showm = window.ShowManager(scene,
size=(900, 700), reset_camera=False,
order_transparent=True)
showm.initialize()
import pybullet as p
import time
import math
usePhysX = True
if usePhysX:
p.connect(p.PhysX)
p.loadPlugin("eglRendererPlugin")
else:
p.connect(p.GUI)
p.loadURDF("plane.urdf")
for i in range (10):
sphere = p.loadURDF("marble_cube.urdf",[0,-1,1+i*1], useMaximalCoordinates=False)
p.changeDynamics(sphere ,-1, mass=1000)
door = p.loadURDF("door.urdf",[0,0,1])
p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
print("numJoints = ", p.getNumJoints(door))
p.setGravity(0,0,-10)
position_control = True
angle = math.pi*0.25
p.resetJointState(door,1,angle)
angleread = p.getJointState(door,1)
print("angleread = ",angleread)
prevTime = time.time()
#for j in range (20):
# for i in range (100):
# if (i<99):
# sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates)
# else:
# orn = p.getQuaternionFromEuler([0,-3.14*0.24,0])
# sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates)
print("loaded!")
#p.changeDynamics(sphere ,-1, mass=1000)
door = p.loadURDF("door.urdf",[0,0,-11])
p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
print("numJoints = ", p.getNumJoints(door))
p.setGravity(0,0,-10)
position_control = True
angle = math.pi*0.25
p.resetJointState(door,1,angle)
angleread = p.getJointState(door,1)
print("angleread = ",angleread)
prevTime = time.time()
angle = math.pi*0.5
count=0
p.connect(p.GUI)
#p.loadURDF("wheel.urdf",[0,0,3])
p.setAdditionalSearchPath(pybullet_data.getDataPath())
plane=p.loadURDF("plane100.urdf",[0,0,0])
timestep = 1./240.
bike=-1
for i in range (1):
bike=p.loadURDF("bicycle/bike.urdf",[0,0+3*i,1.5], [0,0,0,1], useFixedBase=False)
p.setJointMotorControl2(bike,0,p.VELOCITY_CONTROL,targetVelocity=0,force=0.05)
#p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=1000)
p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=0)
p.setJointMotorControl2(bike,2,p.VELOCITY_CONTROL,targetVelocity=15, force=20)
p.changeDynamics(plane,-1, mass=20,lateralFriction=1, linearDamping=0, angularDamping=0)
p.changeDynamics(bike,1,lateralFriction=1,linearDamping=0, angularDamping=0)
p.changeDynamics(bike,2,lateralFriction=1,linearDamping=0, angularDamping=0)
#p.resetJointState(bike,1,0,100)
#p.resetJointState(bike,2,0,100)
#p.resetBaseVelocity(bike,[0,0,0],[0,0,0])
#p.setPhysicsEngineParameter(numSubSteps=0)
#bike=p.loadURDF("frame.urdf",useFixedBase=True)
#bike = p.loadURDF("handlebar.urdf", useFixedBase=True)
#p.loadURDF("handlebar.urdf",[0,2,1])
#coord = p.loadURDF("handlebar.urdf", [0.7700000000000005, 0, 0.22000000000000006],useFixedBase=True)# p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
#coord = p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(0)
#coordPos = [0,0,0]
#coordOrnEuler = [0,0,0]
def get_sim(self, mode='GUI'):
bag = rosbag.Bag('./rosbag_data/2020-12-04-12-41-02.bag')
puck_poses, _, _, self.t = self.read_bag(bag)
if mode == "GUI":
p.connect(p.GUI, 1234) # use build-in graphical user interface, p.DIRECT: pass the final results
p.resetDebugVisualizerCamera(cameraDistance=0.45, cameraYaw=-90.0, cameraPitch=-89,
cameraTargetPosition=[1.55, 0.85, 1.])
elif mode == 'DIRECT':
p.connect(p.DIRECT)
else:
print('Input wrong simulation mode ')
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setGravity(0., 0., -9.81)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
file = os.path.join(os.path.dirname(os.path.abspath(path_robots)), "models", "air_hockey_table", "model.urdf")
self.table = p.loadURDF(file, [1.7, 0.85, 0.117], [0, 0, 0.0, 1.0])
file = os.path.join(os.path.dirname(os.path.abspath(path_robots)), "models", "puck", "model.urdf")
# self.puck = p.loadURDF(file, puck_poses[0, 0:3], [0, 0, 0.0, 1.0], flags=p.URDF_USE_IMPLICIT_CYLINDER)
self.puck = p.loadURDF(file, puck_poses[0, 0:3], [0, 0, 0.0, 1.0])
j_puck = self.get_joint_map(self.puck)
j_table = self.get_joint_map(self.table)
tablef = 1
p.changeDynamics(self.table, j_table.get(b'base_joint'), lateralFriction=tablef)
p.changeDynamics(self.table, j_table.get(b'base_down_rim_l'), lateralFriction=tablef)
p.changeDynamics(self.table, j_table.get(b'base_down_rim_r'), lateralFriction=tablef)
p.changeDynamics(self.table, j_table.get(b'base_down_rim_top'), lateralFriction=tablef)
p.changeDynamics(self.table, j_table.get(b'base_left_rim'), lateralFriction=tablef)
p.changeDynamics(self.table, j_table.get(b'base_right_rim'), lateralFriction=tablef)
p.changeDynamics(self.table, j_table.get(b'base_up_rim_l'), lateralFriction=tablef)
p.changeDynamics(self.table, j_table.get(b'base_up_rim_r'), lateralFriction=tablef)
p.changeDynamics(self.table, j_table.get(b'base_up_rim_top'), lateralFriction=tablef)
p.changeDynamics(self.puck, -1, lateralFriction= 0.1)
puck_posori_6, puck_posori_2 = self.initdata(puck_poses)
speed, t_series = self.Diff(puck_posori_2, 10, 0.4)
posestart = puck_poses[354:, :]
tan_theta = (posestart[25, :2] - posestart[0, :2])[1] / (posestart[25, :2] - posestart[0, :2])[0]
cos_theta = 1 / np.sqrt(np.square(tan_theta) + 1)
sin_theta = tan_theta / np.sqrt(np.square(tan_theta) + 1)
initori = [cos_theta, sin_theta]
linvel = np.zeros((2,))
for i, s in enumerate(speed[:, 0]):
if s > 2.9:
linvel = speed[i, 0]
angvel = speed[i, 1]
break
init_linvel = np.hstack((linvel * np.array(initori), 0))
init_angvel = np.hstack(([0, 0], 0))
poses_pos = []
poses_ang = []
simvel = []
readidx = 0
###################### run bag ########################################
# self.runbag(readidx, posestart)
# p.setRealTimeSimulation(1)
# p.setPhysicsEngineParameter(fixedTimeStep=t_series[-1]/len(t_series))
p.resetBasePositionAndOrientation(self.puck, posestart[readidx, 0:3], posestart[readidx, 3:7])
p.resetBaseVelocity(self.puck, linearVelocity=init_linvel) #, angularVelocity=init_angvel)
p.setTimeStep(1 / 120)
while readidx < speed.shape[0]:
p.stepSimulation()
time.sleep(1/120.)
self.collision_filter(self.puck, self.table)
if readidx == 0:
lastpuck = posestart[readidx, 0:3]
poses_pos.append(lastpuck)
poses_ang.append(posestart[readidx, 3:7])
readidx += 1
simvel.append(p.getBaseVelocity(self.puck)[0] + p.getBaseVelocity(self.puck)[1])
recordpos, recordang = p.getBasePositionAndOrientation(self.puck)
poses_pos.append(recordpos)
poses_ang.append(recordang)
if mode == 'GUI':
p.addUserDebugLine(lastpuck, recordpos, lineColorRGB=[0.1, 0.1, 0.5], lineWidth=5)
lastpuck = recordpos
readidx += 1
poses_ang = np.array(poses_ang)
for i in range(poses_ang.shape[0]):
poses_ang[i, :3] = p.getEulerFromQuaternion(poses_ang[i, :])
poses_ang = poses_ang[:, :3]
p.disconnect()
return t_series, np.hstack((np.array(poses_pos), np.array(poses_ang))), puck_posori_6
velocity = 1
num = 40
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.setPhysicsEngineParameter(enableConeFriction=1)
for i in range (num):
print("progress:",i,num)
x = velocity*math.sin(2.*3.1415*float(i)/num)
y = velocity*math.cos(2.*3.1415*float(i)/num)
print("velocity=",x,y)
sphere=p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphere,-1,lateralFriction=0.02)
#p.changeDynamics(sphere,-1,rollingFriction=10)
p.changeDynamics(sphere,-1,linearDamping=0)
p.changeDynamics(sphere,-1,angularDamping=0)
p.resetBaseVelocity(sphere,linearVelocity=[x,y,0])
prevPos = [0,0,0]
for i in range (2048):
p.stepSimulation()
pos = p.getBasePositionAndOrientation(sphere)[0]
if (i&64):
p.addUserDebugLine(prevPos,pos,[1,0,0],1)
prevPos = pos
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
halfpi = 1.57079632679
twopi = 4 * halfpi
kneeangle = -2.1834
dyn = p.getDynamicsInfo(quadruped, -1)
mass = dyn[0]
friction = dyn[1]
localInertiaDiagonal = dyn[2]
print("localInertiaDiagonal", localInertiaDiagonal)
#this is a no-op, just to show the API
p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal)
#for i in range (nJoints):
# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
drawInertiaBox(quadruped, -1, [1, 0, 0])
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
for i in range(nJoints):
drawInertiaBox(quadruped, i, [0, 1, 0])
if (useMaximalCoordinates):
steps = 400
for aa in range(steps):
p.setJointMotorControl2(quadruped, motor_front_leftL_joint,
p.POSITION_CONTROL,
useMaximalCoordinates = True
sphereRadius = 0.005
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1
visualShapeId = -1
for i in range (5):
for j in range (5):
for k in range (5):
#if (k&2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*5*sphereRadius,j*5*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates)
#else:
# sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
p.changeDynamics(sphereUid,-1,ccdSweptSphereRadius=0.002)
p.setGravity(0,0,-10)
pts = p.getContactPoints()
print("num points=",len(pts))
print(pts)
while (p.isConnected()):
time.sleep(1./240.)
p.stepSimulation()
if(nozzleTgt) < 0:
lineEnd = np.dot(RCSThrust,-1)
else:
lineEnd = RCSThrust
lineRot = rot.from_quat(lineOrn)
lineEnd = lineRot.apply(lineEnd)
p.addUserDebugLine(thrustlineStart,thrustlineStart+thrustlineEnd,[1,0.5,0],5,1/20)
p.addUserDebugLine(lineStart,lineStart+lineEnd,[1,0.5,0],2,1/20)
"""
#Update Fuel State
fuelMass = p.getDynamicsInfo(rocket,0)
fuelMass = fuelMass[0] - (mdot/timestep)
p.changeDynamics(rocket, 0, mass=fuelMass)
pos = -1+(fuelMass/10)
p.setJointMotorControl(rocket,0,p.POSITION_CONTROL,pos)
fuelLength = 2*fuelMass/10
fuelInertia = [1/12*fuelMass*(3*0.2*0.2+fuelLength*fuelLength),
1/12*fuelMass*(3*0.2*0.2+fuelLength*fuelLength),
1/2*fuelMass*0.2*0.2]
p.changeDynamics(rocket,0,localInertiaDiagonal=fuelInertia)
#Simulate
p.resetDebugVisualizerCamera(4,30,-30,rocketPos)
p.stepSimulation()
for i in range(segmentLength):
boxId = p.createMultiBody(0,
colSphereId,
-1, [segmentStart, 0, -0.1],
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)):
targetVelocity = 10
if (i % 2):
targetVelocity = -10
p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=100)
segmentStart = segmentStart - 1.1
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1):
camData = p.getDebugVisualizerCamera()
time.sleep(1./240.) # Time in seconds. (premade_time(0.01)) # for i in range(0,1000): # # Simulation and Params # p.stepSimulation() # time.sleep(1./240.) # # p.setTimeStep() # Read - Viz on GUI incline_frict = p.readUserDebugParameter(inclineUI) r_frict = p.readUserDebugParameter(r_boxUI) w_frict = p.readUserDebugParameter(w_boxUI) # spinningFriction = p.readUserDebugParameter(spinningFrictionId) # Dynamics_GUI p.changeDynamics(incline,0, lateralFriction = 0.5) p.changeDynamics(r_box, -1, lateralFriction = r_frict) p.changeDynamics(w_box, -1, lateralFriction = w_frict) # Get Dynamics Info - Get Camera Viz - Write - Debug pos, orn = p.getBasePositionAndOrientation(r_box) pos1, orn1 = p.getBasePositionAndOrientation(w_box) r_box_velocity = p.getBaseVelocity(r_box, ) w_box_Velocity = p.getBaseVelocity(w_box, ) # It's for Link Description, info = p.getDynamicsInfo(w_box,-1) # getDebugVisualizerCamera viewMatrix=[10., 10., 10., 1] or view_matrixRPY k = p.getCameraImage(width=Render_width, height=Render_height, viewMatrix=view_matrix, projectionMatrix=fov_project_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) img_rgb = cv2.cvtColor(k[2], cv2.COLOR_BGR2RGB)
flags = p.URDF_ENABLE_SLEEPING
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.loadURDF("plane100.urdf", flags=flags, useMaximalCoordinates=useMaximalCoordinates)
#p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags)
r2d2 = -1
for k in range(5):
for i in range(5):
r2d2 = p.loadURDF("r2d2.urdf", [k * 2, i * 2, 1],
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES + flags)
#enable sleeping: you can pass the flag during URDF loading, or do it afterwards
#p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
for j in range(p.getNumJoints(r2d2)):
p.setJointMotorControl2(r2d2, j, p.VELOCITY_CONTROL, targetVelocity=0)
print("r2d2=", r2d2)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
timestep = 1. / 240.
p.setTimeStep(timestep)
p.setGravity(0, 0, -10)
while p.isConnected():
p.stepSimulation()
time.sleep(timestep)
#force the object to wake up
p.changeDynamics(r2d2, -1, activationState=p.ACTIVATION_STATE_WAKE_UP)
def __init__(self, state, adr, debug=False, render=False):
# --- Step related ---
self.state = state
self.adr = adr
self.timeStep = 1 / 240
self.frameSkip = 8
self.lowpass_joint_f = 15 # Hz
self.lowpass_joint_alpha = min(1, self.timeStep * self.lowpass_joint_f)
self.lowpass_rew_f = 5 # 5 # Hz
self.lowpass_rew_alpha = min(
1, self.timeStep * self.lowpass_rew_f * self.frameSkip)
# --- Render-related ---
self.debug = debug
self.render = render
self.first_render = True
self.render_path = None
self.raw_frames = []
self.frame = 0
self.frame_per_render = 4
# --- Connecting to the right server ---
if self.debug:
self.pcId = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1, 0, 0, [0, 0, 0.3])
#p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
self.to_plot = [[] for i in range(100)]
else:
self.pcId = p.connect(p.DIRECT)
# --- Loading the meshes ---
urdf_path = str(Path(__file__).parent) + "/urdf"
self.groundId = p.loadURDF(urdf_path + "/plane_001/plane.urdf",
physicsClientId=self.pcId)
#self.robotId = p.loadURDF(urdf_path + "/robot_001/robot.urdf", [0,0,1], physicsClientId=self.pcId)
#self.robotId = p.loadURDF(urdf_path + "/robot_002_1/robot.urdf", [0,0,1], physicsClientId=self.pcId)
self.robotId = p.loadURDF(urdf_path + "/robot_002_1/robot.urdf",
[0, 0, 0],
flags=p.URDF_MERGE_FIXED_LINKS,
physicsClientId=self.pcId)
self.urdf_joint_indexes = [0, 1, 2, 6, 7, 8, 9, 10, 11, 3, 4, 5]
#self.urdf_joint_indexes = [2,4,8,13,15,19,24,26,30,35,37,41]
p.setPhysicsEngineParameter(fixedTimeStep=self.timeStep,
physicsClientId=self.pcId)
# --- setting the right masses ---
p.changeDynamics(self.robotId, -1, mass=6.7, physicsClientId=self.pcId)
for i in range(p.getNumJoints(self.robotId,
physicsClientId=self.pcId)):
mass = p.getDynamicsInfo(self.robotId,
i,
physicsClientId=self.pcId)[0]
p.changeDynamics(self.robotId,
i,
mass=mass * 2 / 1.789,
physicsClientId=self.pcId)
if False:
for i in range(-1, 12):
print("mass of link {} : {}".format(
i,
p.getDynamicsInfo(self.robotId,
i,
physicsClientId=self.pcId)[0]))
# --- setting up the adr ---
self.default_kp = 60 # 108
self.min_kp = self.default_kp * 0.8
self.max_kp = self.default_kp * 1.2
self.min_knee_dkp = self.default_kp * 0.6
self.adr.add_param("min_kp", self.default_kp, -1, self.min_kp)
self.adr.add_param("max_kp", self.default_kp, 1, self.max_kp)
self.adr.add_param("knee_min_kp", self.default_kp, -1,
self.min_knee_dkp)
self.adr.add_param("max_offset_force", 0, 0.1, 100)
self.adr.add_param("max_perturb_force", 0, 1, 1000)
self.adr.add_param("min_friction", 0.7, -1 / 1000, 0.1)
self.band = []
flags=p.URDF_USE_INERTIA_FROM_FILE,
physicsClientId=pc_id)
p.setGravity(0, 0, -9.81, physicsClientId=pc_id)
JOINT_DAMPING = 0.0
if JOINT_DAMPING > 0:
use_damping = True
else:
use_damping = False
# need to be careful with joint damping to zero, because in pybullet the forward dynamics (used for simulation)
# does use joint damping, but the inverse dynamics call does not use joint damping
for link_idx in range(8):
p.changeDynamics(robot_id,
link_idx,
linearDamping=0.0,
angularDamping=0.0,
jointDamping=JOINT_DAMPING,
physicsClientId=pc_id)
p.changeDynamics(robot_id,
link_idx,
maxJointVelocity=200,
physicsClientId=pc_id)
def sample_test_case(robot_model, zero_vel=False, zero_acc=False):
limits_per_joint = robot_model.get_joint_limits()
joint_lower_bounds = [joint["lower"] for joint in limits_per_joint]
joint_upper_bounds = [joint["upper"] for joint in limits_per_joint]
joint_velocity_limits = [joint["velocity"] for joint in limits_per_joint]
joint_angles = []
joint_velocities = []
def reset(self, des_v, des_clear, legs_angle):
self.init_floating = self.state.config[
'init_floating'] if 'init_floating' in self.state.config else False
if not self.init_floating:
p.setGravity(0, 0, -9.81, physicsClientId=self.pcId)
if 'sim_per_step' in self.state.config:
self.frameSkip = self.state.config['sim_per_step']
h0 = 5
self.state.reset()
if self.init_floating:
self.state.base_rot = [0.2, 0, 0, 0.9]
p.resetBasePositionAndOrientation(self.robotId, [0, 0, h0],
self.state.base_rot,
physicsClientId=self.pcId)
p.resetBaseVelocity(self.robotId,
self.state.base_pos_speed,
self.state.base_rot_speed,
physicsClientId=self.pcId)
for i in range(12):
legs_angle[i] += self.state.joint_offset[i]
self.state.joint_rot[i] = legs_angle[i]
self.state.joint_target[i] = legs_angle[i]
self.state.mean_joint_rot[i] = legs_angle[i]
urdf_joint_id = self.urdf_joint_indexes[i]
p.resetJointState(self.robotId,
urdf_joint_id,
legs_angle[i],
0,
physicsClientId=self.pcId)
act_clear = self.get_clearance()
h = des_clear - act_clear + h0 + (0.2 if self.init_floating else 0)
self.state.base_pos = [0, 0, h]
self.state.base_pos_speed = [des_v, 0, 0]
p.resetBasePositionAndOrientation(self.robotId,
self.state.base_pos,
self.state.base_rot,
physicsClientId=self.pcId)
p.resetBaseVelocity(self.robotId,
self.state.base_pos_speed,
self.state.base_rot_speed,
physicsClientId=self.pcId)
# --- friction ---
min_friction = self.adr.value("min_friction")
self.state.friction = np.random.uniform(
min_friction,
0.9) # np.random.uniform(0.7, 2) # 0.6 # np.random.uniform(0.4, 1)
if self.adr.is_test_param("min_friction"):
self.state.friction = min_friction
friction = self.state.friction
restitution = 0.95 # 0.95 # np.random.random()*0.95
#all_foot_id = [9, 20, 31, 42]
all_foot_id = [2, 5, 8, 11]
#for foot_id in all_foot_id:
for foot_id in range(12):
for i in [0]: #[-1, 0, 1]:
urdf_joint_id = foot_id + i
p.changeDynamics(self.robotId,
urdf_joint_id,
lateralFriction=friction,
restitution=restitution,
physicsClientId=self.pcId)
p.setDebugObjectColor(self.robotId,
5, (0, 0, 1),
physicsClientId=self.pcId)
#print(p.getDynamicsInfo(self.groundId, -1, physicsClientId=self.pcId)[5])
p.changeDynamics(self.groundId,
-1,
lateralFriction=friction,
restitution=restitution,
physicsClientId=self.pcId)
# --- joint kp ---
for i in range(12):
cur_max_kp = self.adr.value("max_kp")
min_name = "knee_min_kp" if i % 3 == 2 else "min_kp"
cur_min_kp = self.adr.value(min_name)
kp = np.random.random() * (cur_max_kp - cur_min_kp) + cur_min_kp
if self.adr.is_test_param("max_kp"):
kp = cur_max_kp
if self.adr.is_test_param(min_name):
kp = cur_min_kp
self.state.all_kp[i] = kp
# --- external force ---
perturb_force_norm = self.adr.value("max_offset_force")
if not self.adr.is_test_param("max_offset_force"):
perturb_force_norm *= np.random.random()
found = False
while not found:
offset_force = np.random.normal(size=3)
norm_2 = np.sum(np.square(offset_force))
if norm_2 >= 1e-5:
found = True
self.state.offset_force = offset_force * perturb_force_norm / np.sqrt(
norm_2)
self.state.offset_force = [
self.state.offset_force[i] for i in range(3)
]
self.state.delay = 3 + np.random.randint(5)
"JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR",
"JOINT_FIXED"
]
humanoid = p.loadURDF("humanoid/humanoid.urdf", globalScaling=0.25)
for j in range(p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid, j)
#print(ji)
print("joint[", j, "].type=", jointTypes[ji[2]])
print("joint[", j, "].name=", ji[1])
jointIds = []
paramIds = []
for j in range(p.getNumJoints(humanoid)):
p.changeDynamics(humanoid, j, linearDamping=0, angularDamping=0)
p.changeVisualShape(humanoid, j, rgbaColor=[1, 1, 1, 1])
info = p.getJointInfo(humanoid, j)
#print(info)
if (not useMotionCapture):
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))
print("jointName=", jointName, "at ", j)
p.changeVisualShape(humanoid, 14, rgbaColor=[1, 0, 0, 1])
chest = 1
neck = 2
def change_all_bead_dynamics(self, kwargs):
for droplet in self.droplets:
p.changeDynamics(droplet, -1, **kwargs)
basePosition,
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(sphereUid,
-1,
spinningFriction=0.001,
rollingFriction=0.001,
linearDamping=0.0)
for joint in range(p.getNumJoints(sphereUid)):
p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
p.getNumJoints(sphereUid)
for i in range(p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid, i)
while (1):
keys = p.getKeyboardEvents()
print(keys)
colSphereId,
-1, [segmentStart, 0, -0.1],
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(boxId,
-1,
spinningFriction=0.001,
rollingFriction=0.001,
linearDamping=0.0)
print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)):
targetVelocity = 10
if (i % 2):
targetVelocity = -10
p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=100)
segmentStart = segmentStart - 1.1
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
jointOffsets.append(0)
if i>=2:
dir_flag=-1
jointOffsets.append(dir_flag*0)
maxForceId = p.addUserDebugParameter("maxForce",0,500,400)
restitutionId = p.addUserDebugParameter("restitution",0,1,0.1)
restitutionThresholdId = p.addUserDebugParameter("res. vel. thres",0,100,100)
p.getCameraImage(480,320)
p.setRealTimeSimulation(1)
# while(1):
for j in range (p.getNumJoints(quadruped)):
restitution = p.readUserDebugParameter(restitutionId)
restitutionThreshold = p.readUserDebugParameter(restitutionThresholdId)
p.setPhysicsEngineParameter(restitutionVelocityThreshold = restitutionThreshold)
p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0)
p.changeDynamics(plane, -1, restitution=restitution)
p.changeDynamics(quadruped, j, restitution=restitution)
p.changeDynamics(plane, -1, lateralFriction=1)
p.changeDynamics(quadruped,j,lateralFriction=1)
info = p.getJointInfo(quadruped,j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j)
if (jointType==p.JOINT_FIXED):
p.changeDynamics(quadruped,j,lateralFriction=1)
# restitutionThreshold = p.readUserDebugParameter(restitutionThresholdId)
# p.changeDynamics(plane, -1, restitution=restitution)
p.changeDynamics(quadruped,j,rollingFriction=0.2)
def __init__(
self,
gui=True,
gravity=(0, 0, -9),
use_realistic_speed=True,
en_sway=True,
):
self.use_realistic_speed = use_realistic_speed
self.en_sway = en_sway
if gui:
self.connection_mode = p.GUI
else:
self.connection_mode = p.DIRECT
self.cnt = 0
physicsClient = p.connect(self.connection_mode)
# do not use stupid urdf caching which shits the bed whenever urdf is
# changed
p.setPhysicsEngineParameter(enableFileCaching=0, useSplitImpulse=0)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
#p.setRealTimeSimulation(enableRealTimeSimulation=1)
# disable debug windows
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
self.gravity = gravity
p.setGravity(*self.gravity)
planeId = p.loadURDF("plane.urdf") # noqa
cube_start_pos = [0, 0, 0]
cube_start_orientation = p.getQuaternionFromEuler([0, 0, 0])
if self.en_sway:
crane_urdf_path = "models/crane.urdf"
else:
crane_urdf_path = "models/crane_nosway.urdf"
print(crane_urdf_path)
logstash_urdf_path = pkg_resources.resource_filename(
__name__, "models/logs.urdf"
)
self.crane_body_id = p.loadURDF(
crane_urdf_path, cube_start_pos, cube_start_orientation #,flags=p.URDF_USE_SELF_COLLISION#, useFixedBase=1
)
#self.log_stash = p.loadURDF(
# logstash_urdf_path, cube_start_pos, cube_start_orientation
#)
self.num_joints = p.getNumJoints(self.crane_body_id)
#for joint in range(0, self.num_joints):
# p.setCollisionFilterPair(self.crane_body_id, self.log_stash, joint, 1, 1)
# link name -> link index
self._link_map = {}
# joint name -> joint index
self._joint_map = {}
####################################################################################
# print list of all joints and create mappings name -> index for links and joints #
####################################################################################
print(
"List of all joints (Joint Index is the Link Index belonging to Child Link Name)"
)
header_template = (
"{joint_idx:^20}|{joint_name:^40}|{link_name:^40}|{parent_idx:^20}"
)
header = header_template.format(
joint_idx="Joint Index",
joint_name="Joint Name",
link_name="Child Link Name",
parent_idx="Parent Link Index",
)
print(header)
print("-" * len(header))
for joint in range(self.num_joints):
joint_info = p.getJointInfo(self.crane_body_id, joint)
joint_name = joint_info[1].decode("utf-8")
link_name = joint_info[12].decode("utf-8")
parent_idx = joint_info[16]
print("{joint:^20}|{joint_name:^40}|{link_name:^40}|{parent_idx:^20}".format(joint=joint, joint_name=joint_name,link_name=link_name,parent_idx=parent_idx))
self._link_map[link_name] = joint
self._joint_map[joint_name] = joint
if self.en_sway:
p.setJointMotorControl2(
self.crane_body_id,
self._joint_map["trolley_to_sway_x"],
p.POSITION_CONTROL,
force=1,
)
p.setJointMotorControl2(
self.crane_body_id,
self._joint_map["sway_x_to_sway_y"],
p.POSITION_CONTROL,
force=1,
)
p.changeDynamics(
self.crane_body_id,
self._link_map["sway_link_x"],
lateralFriction=1,
spinningFriction=1,
rollingFriction=1,
contactStiffness=10000,
contactDamping=0,
frictionAnchor=10000,
)
p.changeDynamics(
self.crane_body_id,
self._link_map["sway_link_y"],
lateralFriction=1,
spinningFriction=1,
rollingFriction=1,
contactStiffness=10000,
contactDamping=0,
frictionAnchor=10000,
)
### for issue demonstration purpose only
### normally action generation is handled via a zmq connection
self.joint_names = ["gantry", "trolley", "hoist", "grapple", "tines"]
self.current_speed = OrderedDict(
zip(self.joint_names, [0.0, 0.0, 0.0, 0.0, 0.0])
)
self.desired_speed = OrderedDict(
zip(self.joint_names, [0.0, 0.0, 0.0, 0.0, 0.0])
)
self.speed_limit = OrderedDict(
zip(self.joint_names, [2.03, 0.76, 0.37, 1.0, 1.0])
)
self.speed_step = OrderedDict(
zip(self.joint_names, [0.01, 0.005, 0.005, 1.0, 1.0])
)
self.last_known_encoders = None
self.step_counter = 0
restitutionVelocityThresholdId = p.addUserDebugParameter("res. vel. threshold", 0, 3, 0.2)
lateralFrictionId = p.addUserDebugParameter("lateral friction", 0, 1, 0.5)
spinningFrictionId = p.addUserDebugParameter("spinning friction", 0, 1, 0.03)
rollingFrictionId = p.addUserDebugParameter("rolling friction", 0, 1, 0.03)
plane = p.loadURDF("plane_with_restitution.urdf")
sphere = p.loadURDF("sphere_with_restitution.urdf", [0, 0, 2])
p.setRealTimeSimulation(1)
p.setGravity(0, 0, -10)
while (1):
restitution = p.readUserDebugParameter(restitutionId)
restitutionVelocityThreshold = p.readUserDebugParameter(restitutionVelocityThresholdId)
p.setPhysicsEngineParameter(restitutionVelocityThreshold=restitutionVelocityThreshold)
lateralFriction = p.readUserDebugParameter(lateralFrictionId)
spinningFriction = p.readUserDebugParameter(spinningFrictionId)
rollingFriction = p.readUserDebugParameter(rollingFrictionId)
p.changeDynamics(plane, -1, lateralFriction=1)
p.changeDynamics(sphere, -1, lateralFriction=lateralFriction)
p.changeDynamics(sphere, -1, spinningFriction=spinningFriction)
p.changeDynamics(sphere, -1, rollingFriction=rollingFriction)
p.changeDynamics(plane, -1, restitution=restitution)
p.changeDynamics(sphere, -1, restitution=restitution)
pos, orn = p.getBasePositionAndOrientation(sphere)
#print("pos=")
#print(pos)
time.sleep(0.01)
def run():
pb.connect(pb.GUI)
pb.setAdditionalSearchPath(os.path.abspath('..') + '/models')
pb.setGravity(0, 0, -9.8)
pb.loadURDF("track_straight.urdf", [40, 0, 0],
flags=pb.URDF_USE_INERTIA_FROM_FILE)
cube_start_pos = [0, 2.5, 3]
cube_start_orientation = pb.getQuaternionFromEuler([0, 0, 0])
car_id = pb.loadURDF("model.urdf",
cube_start_pos,
cube_start_orientation,
flags=pb.URDF_USE_IMPLICIT_CYLINDER
or pb.URDF_USE_INERTIA_FROM_FILE)
car2_id = pb.loadURDF("model.urdf", [0, -2.5, 3],
cube_start_orientation,
flags=pb.URDF_USE_IMPLICIT_CYLINDER
or pb.URDF_USE_INERTIA_FROM_FILE)
print("dynamicsInfo:", pb.getDynamicsInfo(car_id, 2))
pb.changeDynamics(car_id, -1, linearDamping=0, angularDamping=0)
pb.changeDynamics(car2_id, -1, linearDamping=0, angularDamping=0)
joint_num = pb.getNumJoints(car_id)
joint_index = []
for i in range(joint_num):
joint_index.append(pb.getJointInfo(car_id, i))
pb.setJointMotorControlArray(car_id, [2, 4, 5, 6],
pb.VELOCITY_CONTROL,
targetVelocities=[0, 0, 0, 0],
forces=[0, 0, 0, 0])
pb.setJointMotorControlArray(car2_id, [2, 4, 5, 6],
pb.VELOCITY_CONTROL,
targetVelocities=[0, 0, 0, 0],
forces=[0, 0, 0, 0])
velocity = 0
velocity2 = 0
force = 1500
pb.setJointMotorControlArray(car_id, [1, 3],
pb.POSITION_CONTROL,
targetPositions=[0, 0])
pb.setJointMotorControlArray(
car_id, [2, 4, 5, 6],
pb.VELOCITY_CONTROL,
targetVelocities=[velocity, velocity, velocity, velocity],
forces=[force, force, force, force])
pb.setJointMotorControlArray(car2_id, [1, 3],
pb.POSITION_CONTROL,
targetPositions=[0, 0])
pb.setJointMotorControlArray(
car2_id, [2, 4, 5, 6],
pb.VELOCITY_CONTROL,
targetVelocities=[velocity2, velocity2, velocity2, velocity2],
forces=[force, force, force, force])
pb.setRealTimeSimulation(True)
# add_force(-100000, -100000000, -10000, 0.2)
# Ray Sensor Start===========================================================
ray_front_base_start_pos = np.array([2.6, 0, 0])
ray_rear_base_start_pos = np.array([-2.6, 0, 0])
ray_front_base_end_pos = np.array([20, 0, 0])
ray_rear_base_end_pos = np.array([-20, 0, 0])
ray_front_base_end_pos_list = []
ray_rear_base_end_pos_list = []
for i in range(-6, 7, 1):
ray_front_base_end_pos_list.append(
utils.pos_ori_dist_to_abs_pos_np(
ray_front_base_start_pos,
utils.get_quat_from_base_var([0, 0, 1, (90 / 6) * i]),
ray_front_base_end_pos))
ray_rear_base_end_pos_list.append(
utils.pos_ori_dist_to_abs_pos_np(
ray_rear_base_start_pos,
utils.get_quat_from_base_var([0, 0, 1, (90 / 6) * i]),
ray_rear_base_end_pos))
ray_line_ids = []
for i in range(29): # 13 front 13 back 3 bottom
ray_line_ids.append(
pb.addUserDebugLine([0, 0, 0], [0, 0, 0], [0.0944, 0.6389, 0]))
# MAIN LOOP--------------------------------------------------------------------------------------
time.sleep(1)
for i in range(30000):
car_pos, car_ori = pb.getBasePositionAndOrientation(car_id)
car_pos, car_ori = np.array(car_pos), np.array(car_ori)
ray_front_start_pos = utils.pos_ori_dist_to_abs_pos_np(
car_pos, car_ori, ray_front_base_start_pos)
ray_rear_start_pos = utils.pos_ori_dist_to_abs_pos_np(
car_pos, car_ori, ray_rear_base_start_pos)
ray_bottom_start_pos_list = []
ray_bottom_end_pos_list = []
ray_front_end_pos_list = []
ray_rear_end_pos_list = []
for j in range(13):
if j < 3: # j is > 1 on the last 3 loop
ray_bottom_start_pos_list.append(
utils.pos_ori_dist_to_abs_pos_np(
car_pos, car_ori, [0, 1.2 * (j - 1), -0.06]))
ray_bottom_end_pos_list.append(
utils.pos_ori_dist_to_abs_pos_np(car_pos, car_ori,
[0, 4 * (j - 1), -2.5]))
ray_front_end_pos_list.append(
utils.pos_ori_dist_to_abs_pos_np(
car_pos, car_ori, ray_front_base_end_pos_list[j]))
ray_rear_end_pos_list.append(
utils.pos_ori_dist_to_abs_pos_np(
car_pos, car_ori, ray_rear_base_end_pos_list[j]))
ray_results = pb.rayTestBatch(
[ray_front_start_pos
for _ in range(13)] + [ray_rear_start_pos for _ in range(13)] +
ray_bottom_start_pos_list, ray_front_end_pos_list +
ray_rear_end_pos_list + ray_bottom_end_pos_list)
for j in range(13): # draw debug line for ray sensors
ray_line_ids[j] = pb.addUserDebugLine(
ray_front_start_pos,
ray_front_end_pos_list[j], [0.0944, 0.6389, 0],
replaceItemUniqueId=ray_line_ids[j])
ray_line_ids[j + 13] = pb.addUserDebugLine(
ray_rear_start_pos,
ray_rear_end_pos_list[j], [0.0944, 0.6389, 0],
replaceItemUniqueId=ray_line_ids[j + 13])
if j < 3:
ray_line_ids[j + 26] = pb.addUserDebugLine(
ray_bottom_start_pos_list[j],
ray_bottom_end_pos_list[j], [0.0944, 0.6389, 0],
replaceItemUniqueId=ray_line_ids[j + 26])
ray_distance_result = []
for j in range(29):
ray_distance_result.append(ray_results[j][2])
# print("ray_distance_results:", ray_distance_result)
# Ray Sensor End=============================================================
# force = FORCE
# pb.setJointMotorControlArray(
# car_id, [2, 4, 5, 6], pb.TORQUE_CONTROL,
# forces=[force, force, force, force])
# pb.setJointMotorControlArray(
# car2_id, [2, 4, 5, 6], pb.TORQUE_CONTROL,
# forces=[force, force, force, force])
# contact_results = pb.getContactPoints()
# print("Normal force:", contact_results[0][9])
# print("Lateral friction1:", contact_results[0][10])
# print("Lateral friction1 direction:", contact_results[0][11])
# print("Lateral friction2:", contact_results[0][12])
# print("Lateral friction2 direction:", contact_results[0][13], "\n")
# time.sleep(1)
pb.disconnect()
minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True)
print(p.getNumJoints(minitaur))
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=23.2,
cameraPitch=-6.6,
cameraTargetPosition=[-0.064, .621, -0.2])
motorJointId = 1
p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0)
p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1)
angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0)
jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0)
textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2])
p.setRealTimeSimulation(1)
while (1):
frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
angularDamping = p.readUserDebugParameter(angularDampingSlider)
p.setJointMotorControl2(minitaur,
motorJointId,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=frictionForce)
p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping)
time.sleep(0.01)
txt = "jointVelocity=" + str(p.getJointState(minitaur, motorJointId)[1])
prevTextId = textId
textId = p.addUserDebugText(txt, [0, 0, -0.2])
p.removeUserDebugItem(prevTextId)
urdf_path,
basePosition=[0, 0, 0],
useFixedBase=True,
flags=p.URDF_USE_INERTIA_FROM_FILE,
)
p.setGravity(0, 0, -9.81)
JOINT_DAMPING = 0.5
# need to be careful with joint damping to zero, because in pybullet the forward dynamics (used for simulation)
# does use joint damping, but the inverse dynamics call does not use joint damping
for link_idx in range(8):
p.changeDynamics(
robot_id,
link_idx,
linearDamping=0.0,
angularDamping=0.0,
jointDamping=JOINT_DAMPING,
)
p.changeDynamics(robot_id, link_idx, maxJointVelocity=200)
def sample_test_case(robot_model, zero_vel=False, zero_acc=False):
limits_per_joint = robot_model.get_joint_limits()
joint_lower_bounds = [joint["lower"] for joint in limits_per_joint]
joint_upper_bounds = [joint["upper"] for joint in limits_per_joint]
joint_velocity_limits = [joint["velocity"] for joint in limits_per_joint]
joint_angles = []
joint_velocities = []
joint_accelerations = []
import time
p.connect(p.GUI)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM = 0.000001)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
p.loadURDF("plane.urdf")
radius = 0.025
distance = 1.86
yaw=135
pitch=-11
targetPos=[0,0,0]
p.setPhysicsEngineParameter(solverResidualThreshold=0.001, numSolverIterations=200)
p.resetDebugVisualizerCamera(distance, yaw,pitch, targetPos)
objectId = -1
for i in range (10):
objectId = p.loadURDF("cube_small.urdf",[1,1,radius+i*2*radius])
p.changeDynamics(objectId,-1,100)
timeStep = 1./240.
p.setGravity(0,0,-10)
while (p.isConnected()):
p.stepSimulation()
time.sleep(timeStep)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
import pybullet as p
import time
p.connect(p.GUI)
cube2 = p.loadURDF("cube.urdf", [0, 0, 3], useFixedBase=True)
cube = p.loadURDF("cube.urdf", useFixedBase=True)
p.setGravity(0, 0, -10)
timeStep = 1. / 240.
p.setTimeStep(timeStep)
p.changeDynamics(cube2, -1, mass=1)
#now cube2 will have a floating base and move
while (p.isConnected()):
p.stepSimulation()
time.sleep(timeStep)
# if W_DOF == 8:
# joints[0:3:9] = 0
dir_flag = 1
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(0)
if i >= 2:
dir_flag = -1
jointOffsets.append(dir_flag * 1.57)
maxForceId = p.addUserDebugParameter("maxForce", 0, 500, 50)
restitution_coeff = 0
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
p.changeDynamics(plane, -1, restitution=restitution_coeff)
p.changeDynamics(quadruped, j, restitution=restitution_coeff)
info = p.getJointInfo(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
# calibration position
import pybullet as p
import time
p.connect(p.GUI)
door = p.loadURDF("door.urdf")
#linear/angular damping for base and all children=0
p.changeDynamics(door, -1, linearDamping=0, angularDamping=0)
for j in range(p.getNumJoints(door)):
p.changeDynamics(door, j, linearDamping=0, angularDamping=0)
print(p.getJointInfo(door, j))
frictionId = p.addUserDebugParameter("jointFriction", 0, 20, 10)
torqueId = p.addUserDebugParameter("joint torque", 0, 20, 5)
while (1):
frictionForce = p.readUserDebugParameter(frictionId)
jointTorque = p.readUserDebugParameter(torqueId)
#set the joint friction
p.setJointMotorControl2(door, 1, p.VELOCITY_CONTROL, targetVelocity=0, force=frictionForce)
#apply a joint torque
p.setJointMotorControl2(door, 1, p.TORQUE_CONTROL, force=jointTorque)
p.stepSimulation()
time.sleep(0.01)
import pybullet as p
import time
import pybullet_data
#by default, PyBullet runs at 240Hz
p.connect(p.GUI, options="--mp4=\"test.mp4\" --mp4fps=240")
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
p.loadURDF("plane.urdf")
#in 3 seconds, the object travels about 0.5*g*t^2 meter ~ 45 meter.
r2d2 = p.loadURDF("r2d2.urdf", [0, 0, 45])
#disable linear damping
p.changeDynamics(r2d2, -1, linearDamping=0)
p.setGravity(0, 0, -10)
for i in range(3 * 240):
txt = "frame " + str(i)
item = p.addUserDebugText(txt, [0, 1, 0])
p.stepSimulation()
#synchronize the visualizer (rendering frames for the video mp4) with stepSimulation
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
#print("r2d2 vel=", p.getBaseVelocity(r2d2)[0][2])
p.removeUserDebugItem(item)
p.disconnect()
# Switch between URDF with/without FIXED joints
with_fixed_joints = True
if with_fixed_joints:
id_revolute_joints = [0, 3]
id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True)
else:
id_revolute_joints = [0, 1]
id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True)
bullet.changeDynamics(id_robot,-1,linearDamping=0, angularDamping=0)
bullet.changeDynamics(id_robot,0,linearDamping=0, angularDamping=0)
bullet.changeDynamics(id_robot,1,linearDamping=0, angularDamping=0)
jointTypeNames = ["JOINT_REVOLUTE", "JOINT_PRISMATIC","JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED","JOINT_POINT2POINT","JOINT_GEAR"]
# Disable the motors for torque control:
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.VELOCITY_CONTROL, forces=[0.0, 0.0])
# Target Positions:
start = 0.0
end = 1.0
steps = int((end-start)/delta_t)
t = [0]*steps
q_pos_desired = [[0.]* steps,[0.]* steps]
p.setTimeStep(timeStep)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
urdfFlags = p.URDF_USE_SELF_COLLISION
iiwa = p.loadURDF(os.path.abspath("./SrdPy/examples/iiwa/iiwa14.urdf"),
[0, 0, 0.48], [0, 0, 0, 1],
flags=urdfFlags,
useFixedBase=True)
jointIds = []
jointNames = []
for j in range(p.getNumJoints(iiwa)):
p.changeDynamics(iiwa, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(iiwa, j)
jointName = info[1]
jointType = info[2]
if jointType == p.JOINT_REVOLUTE:
jointIds.append(j)
jointNames.append(jointName)
jointNames = [link.joint.name for link in iiwaChain.linkArray[1:]]
jointIds = [id for name, id in zip(jointNames, jointIds) if name in jointNames]
stateHandler = BulletStateHandler(iiwa, jointIds)
gcModelEvaluator = GCModelEvaluatorHandler(handlerGeneralizedCoordinatesModel,
stateHandler)
tf = ikSolutionHandler.timeExpiration
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis,
useMaximalCoordinates=useMaximalCoordinates)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
anistropicFriction = [1, 0.01, 0.01]
p.changeDynamics(sphereUid, -1, lateralFriction=2, anisotropicFriction=anistropicFriction)
p.getNumJoints(sphereUid)
for i in range(p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid, i)
p.changeDynamics(sphereUid, i, lateralFriction=2, anisotropicFriction=anistropicFriction)
dt = 1. / 240.
SNAKE_NORMAL_PERIOD = 0.1 #1.5
m_wavePeriod = SNAKE_NORMAL_PERIOD
m_waveLength = 4
m_wavePeriod = 1.5
m_waveAmplitude = 0.4
m_waveFront = 0.0
#our steering value
m_steering = 0.0
def _step(self, action):
done = False
#reward (float): amount of reward achieved by the previous action. The scale varies between environments, but the goal is always to increase your total reward.
#done (boolean): whether it's time to reset the environment again. Most (but not all) tasks are divided up into well-defined episodes, and done being True indicates the episode has terminated. (For example, perhaps the pole tipped too far, or you lost your last life.)
#observation (object): an environment-specific object representing your observation of the environment. For example, pixel data from a camera, joint angles and joint velocities of a robot, or the board state in a board game.
#info (dict): diagnostic information useful for debugging. It can sometimes be useful for learning (for example, it might contain the raw probabilities behind the environment's last state change). However, official evaluations of your agent are not allowed to use this for learning.
def convertSensor(finger_index):
if finger_index == self.indexId:
return random.uniform(-1, 1)
#return 0
else:
#return random.uniform(-1,1)
return 0
def convertAction(action):
#converted = (action-30)/10
#converted = (action-16)/10
if action == 6:
converted = -1
elif action == 25:
converted = 1
#print("action ",action)
#print("converted ",converted)
return converted
aspect = 1
camTargetPos = [0, 0, 0]
yaw = 40
pitch = 10.0
roll = 0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 240
nearPlane = 0.0001
farPlane = 0.022
lightDirection = [0, 1, 0]
lightColor = [1, 1, 1] #optional
fov = 50 #10 or 50
hand_po = pb.getBasePositionAndOrientation(self.agent)
ho = pb.getQuaternionFromEuler([0.0, 0.0, 0.0])
# So when trying to modify the physics of the engine, we run into some problems. If we leave
# angular damping at default (0.04) then the hand rotates when moving up and dow, due to torque.
# If we set angularDamping to 100.0 then the hand will bounce off into the background due to
# all the stored energy, when it makes contact with the object. The below set of parameters seem
# to have a reasonably consistent performance in keeping the hand level and not inducing unwanted
# behavior during contact.
pb.changeDynamics(self.agent,
linkIndex=-1,
spinningFriction=100.0,
angularDamping=35.0,
contactStiffness=0.0,
contactDamping=100)
if action == 65298 or action == 0: #down
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0] + self.move, hand_po[0][1], hand_po[0][2]),
ho,
maxForce=50)
elif action == 65297 or action == 1: #up
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0] - self.move, hand_po[0][1], hand_po[0][2]),
ho,
maxForce=50)
elif action == 65295 or action == 2: #left
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1] + self.move, hand_po[0][2]),
ho,
maxForce=50)
elif action == 65296 or action == 3: #right
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1] - self.move, hand_po[0][2]),
ho,
maxForce=50)
elif action == 44 or action == 4: #<
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1], hand_po[0][2] + self.move),
ho,
maxForce=50)
elif action == 46 or action == 5: #>
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1], hand_po[0][2] - self.move),
ho,
maxForce=50)
elif action >= 6 and action <= 7:
# keeps the hand from moving towards origin
pb.changeConstraint(self.hand_cid,
(hand_po[0][0], hand_po[0][1], hand_po[0][2]),
ho,
maxForce=50)
if action == 7:
action = 25 #bad kludge redo all this code
"""
self.pinkId = 0
self.middleId = 1
self.indexId = 2
self.thumbId = 3
self.ring_id = 4
pink = convertSensor(self.pinkId) #pinkId != indexId -> return random uniform
middle = convertSensor(self.middleId) # middleId != indexId -> return random uniform
thumb = convertSensor(self.thumbId) # thumbId != indexId -> return random uniform
ring = convertSensor(self.ring_id) # ring_id != indexId -> return random uniform
"""
index = convertAction(
action) # action = 6 or 25 due to kludge -> return -1 or 1
"""
pb.setJointMotorControl2(self.agent,7,pb.POSITION_CONTROL,self.pi/4.)
pb.setJointMotorControl2(self.agent,9,pb.POSITION_CONTROL,thumb+self.pi/10)
pb.setJointMotorControl2(self.agent,11,pb.POSITION_CONTROL,thumb)
pb.setJointMotorControl2(self.agent,13,pb.POSITION_CONTROL,thumb)
"""
# Getting positions of the index joints to use for moving to a relative position
joint17Pos = pb.getJointState(self.agent, 17)[0]
joint19Pos = pb.getJointState(self.agent, 19)[0]
joint21Pos = pb.getJointState(self.agent, 21)[0]
# need to keep the multiplier relatively small otherwise the joint will continue to move
# when you take other actions
finger_jump = 0.2
newJoint17Pos = joint17Pos + index * finger_jump
newJoint19Pos = joint19Pos + index * finger_jump
newJoint21Pos = joint21Pos + index * finger_jump
# following values found by experimentation
if newJoint17Pos <= -0.7:
newJoint17Pos = -0.7
elif newJoint17Pos >= 0.57:
newJoint17Pos = 0.57
if newJoint19Pos <= 0.13:
newJoint19Pos = 0.13
elif newJoint19Pos >= 0.42:
newJoint19Pos = 0.42
if newJoint21Pos <= -0.8:
newJoint21Pos = -0.8
elif newJoint21Pos >= 0.58:
newJoint21Pos = 0.58
pb.setJointMotorControl2(self.agent, 17, pb.POSITION_CONTROL,
newJoint17Pos)
pb.setJointMotorControl2(self.agent, 19, pb.POSITION_CONTROL,
newJoint19Pos)
pb.setJointMotorControl2(self.agent, 21, pb.POSITION_CONTROL,
newJoint21Pos)
"""
pb.setJointMotorControl2(self.agent,17,pb.POSITION_CONTROL,index)
pb.setJointMotorControl2(self.agent,19,pb.POSITION_CONTROL,index)
pb.setJointMotorControl2(self.agent,21,pb.POSITION_CONTROL,index)
pb.setJointMotorControl2(self.agent,24,pb.POSITION_CONTROL,middle)
pb.setJointMotorControl2(self.agent,26,pb.POSITION_CONTROL,middle)
pb.setJointMotorControl2(self.agent,28,pb.POSITION_CONTROL,middle)
pb.setJointMotorControl2(self.agent,40,pb.POSITION_CONTROL,pink)
pb.setJointMotorControl2(self.agent,42,pb.POSITION_CONTROL,pink)
pb.setJointMotorControl2(self.agent,44,pb.POSITION_CONTROL,pink)
ringpos = 0.5*(pink+middle)
pb.setJointMotorControl2(self.agent,32,pb.POSITION_CONTROL,ringpos)
pb.setJointMotorControl2(self.agent,34,pb.POSITION_CONTROL,ringpos)
pb.setJointMotorControl2(self.agent,36,pb.POSITION_CONTROL,ringpos)
"""
if self.downCameraOn: viewMatrix = down_view()
else: viewMatrix = self.ahead_view()
projectionMatrix = pb.computeProjectionMatrixFOV(
fov, aspect, nearPlane, farPlane)
w, h, img_arr, depths, mask = pb.getCameraImage(
200,
200,
viewMatrix,
projectionMatrix,
lightDirection,
lightColor,
renderer=pb.ER_TINY_RENDERER)
#w,h,img_arr,depths,mask = pb.getCameraImage(200,200, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pb.ER_BULLET_HARDWARE_OPENGL)
#red_dimension = img_arr[:,:,0] #TODO change this so any RGB value returns 1, anything else is 0
red_dimension = img_arr[:, :, 0].flatten(
) #TODO change this so any RGB value returns 1, anything else is 0
#observation = red_dimension
self.img_arr = img_arr
observation = (np.absolute(red_dimension - 255) > 0).astype(int)
self.current_observation = observation
self.img_arr = img_arr
self.depths = depths
info = [42] #answer to life,TODO use real values
pb.stepSimulation()
self.steps += 1
#reward if moving towards the object or touching the object
reward = 0
max_steps = 1000
if self.is_touching():
touch_reward = 10
if 'debug' in self.options and self.options['debug'] == True:
print("TOUCHING!!!!")
else:
touch_reward = 0
obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify)
distance = math.sqrt(
sum([(xi - yi)**2 for xi, yi in zip(obj_po[0], hand_po[0])
])) #TODO faster euclidean
#distance = np.linalg.norm(obj_po[0],hand_po[0])
#print("distance:",distance)
if distance < self.prev_distance:
reward += 1 * (max_steps - self.steps)
elif distance > self.prev_distance:
reward -= 10
reward -= distance
reward += touch_reward
self.prev_distance = distance
#print("shape",observation.shape)
if 'debug' in self.options and self.options['debug'] == True:
print("touch reward ", touch_reward)
print("action ", action)
print("reward ", reward)
print("distance ", distance)
if self.steps >= max_steps or self.is_touching():
done = True
return observation, reward, done, info
jointIds=[]
paramIds=[]
jointOffsets=[]
jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1]
jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0]
for i in range (4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce",0,100,20)
for j in range (p.getNumJoints(quadruped)):
p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped,j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480,320)
p.setRealTimeSimulation(0)
joints=[]
with open(pd.getDataPath()+"/laikago/data1.txt","r") as filestream:
for line in filestream:
def __init__(self, dt=0.04, display=True, stars=None, shootingstar=True, seed=None, scope_noise=0.1):
# Create random number generator
self.rng = np.random.default_rng(seed)
# Choose the time step
self.dt = dt
# Connect to and configure pybullet
self.display = display
if self.display:
p.connect(p.GUI, options="--background_color_red=0 --background_color_blue=0 --background_color_green=0")
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
# p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
p.resetDebugVisualizerCamera(6, -90, -45, (0., 0., 0.))
else:
p.connect(p.DIRECT)
p.setPhysicsEngineParameter(fixedTimeStep=self.dt,
numSubSteps=4,
restitutionVelocityThreshold=0.05)
# Load robot
self.robot_id = p.loadURDF(os.path.join('.', 'urdf', 'spacecraft.urdf'),
basePosition=np.array([0., 0., 0.]),
baseOrientation=p.getQuaternionFromEuler([0., 0., 0.]),
useFixedBase=0,
flags=(p.URDF_USE_IMPLICIT_CYLINDER |
p.URDF_USE_INERTIA_FROM_FILE ))
# Load shooting star
self.shootingstar = shootingstar
if self.shootingstar:
self.shot_id = p.loadURDF(os.path.join('.', 'urdf', 'shootingstar.urdf'),
basePosition=np.array([0., 0., 10.]),
baseOrientation=p.getQuaternionFromEuler([0., 0., 0.]),
useFixedBase=0,
flags=(p.URDF_USE_IMPLICIT_CYLINDER |
p.URDF_USE_INERTIA_FROM_FILE ))
p.changeDynamics(self.shot_id, -1, linearDamping=0., angularDamping=0.)
# Eliminate linear and angular damping (a poor model of drag)
p.changeDynamics(self.robot_id, -1, linearDamping=0., angularDamping=0.)
for joint_id in range(p.getNumJoints(self.robot_id)):
p.changeDynamics(self.robot_id, joint_id, linearDamping=0., angularDamping=0.)
# Eliminate joint damping
for joint_index in range(p.getNumJoints(self.robot_id)):
p.changeDynamics(self.robot_id, joint_index, jointDamping=0.)
# Create a dictionary that maps joint names to joint indices
self.joint_map = {}
for joint_index in range(p.getNumJoints(self.robot_id)):
joint_name = p.getJointInfo(self.robot_id, joint_index)[1].decode('UTF-8')
self.joint_map[joint_name] = joint_index
# Create a 1D numpy array with the index (according to bullet) of each joint we care about
self.joint_names = [
'bus_to_wheel_1',
'bus_to_wheel_2',
'bus_to_wheel_3',
'bus_to_wheel_4',
]
self.num_joints = len(self.joint_names)
self.joint_ids = np.array([self.joint_map[joint_name] for joint_name in self.joint_names])
# Disable velocity control on joints so we can use torque control
p.setJointMotorControlArray(self.robot_id, self.joint_ids,
p.VELOCITY_CONTROL, forces=np.zeros(self.num_joints))
# Specify maximum applied torque
self.tau_max = 5.
# Specify maximum wheel speed for non-zero torque
# (20 rad/s is about 200 rpm)
self.v_max = 20.
# Place stars
self.scope_radius = 0.8 / 2.1
self.scope_angle = np.arctan(self.scope_radius)
self.scope_noise = scope_noise
self.star_depth = 5.
if stars is None:
stars = np.array([[0., 0.], [0.15, 0.], [0., 0.15]])
else:
stars = np.array(stars)
if (len(stars.shape) != 2) or (stars.shape[1] != 2):
raise Exception('"stars" must be a numpy array of size n x 2')
self.stars = []
for i in range(stars.shape[0]):
self.stars.append({'alpha': stars[i, 0], 'delta': stars[i, 1],})
for star in self.stars:
star['pos'] = np.array([[np.cos(star['alpha']) * np.cos(star['delta'])],
[np.sin(star['alpha']) * np.cos(star['delta'])],
[np.sin(star['delta'])]]) * self.star_depth
p.loadURDF(os.path.join('.', 'urdf', 'sphere.urdf'),
basePosition=star['pos'].flatten(),
useFixedBase=1)
if self.shootingstar:
for object_id in [self.robot_id, self.shot_id]:
for joint_id in range(-1, p.getNumJoints(object_id)):
p.changeDynamics(object_id, joint_id,
lateralFriction=1.0,
spinningFriction=0.0,
rollingFriction=0.0,
restitution=0.5,
contactDamping=-1,
contactStiffness=-1)
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)
import pybullet as p
import time
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.)