import pybullet as p
from pybullet_utils import gazebo_world_parser
import time
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
gazebo_world_parser.parseWorld(p, filepath="worlds/racetrack_day.world")
shadowIntensityParam = p.addUserDebugParameter("shadowIntensity", 0, 1, 0.8)
p.configureDebugVisualizer(shadowMapResolution=8192)
p.configureDebugVisualizer(shadowMapWorldSize=25)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
dt = 1. / 240.
while (1):
shadowMapIntensity = p.readUserDebugParameter(shadowIntensityParam)
p.configureDebugVisualizer(shadowMapIntensity=shadowMapIntensity)
p.stepSimulation()
p.getCameraImage(640, 480, renderer=p.ER_BULLET_HARDWARE_OPENGL)
time.sleep(dt)
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("plane.urdf")
sphereUid = p.loadURDF("sphere_transparent.urdf", [0, 0, 2])
redSlider = p.addUserDebugParameter("red", 0, 1, 1)
greenSlider = p.addUserDebugParameter("green", 0, 1, 0)
blueSlider = p.addUserDebugParameter("blue", 0, 1, 0)
alphaSlider = p.addUserDebugParameter("alpha", 0, 1, 0.5)
while (1):
red = p.readUserDebugParameter(redSlider)
green = p.readUserDebugParameter(greenSlider)
blue = p.readUserDebugParameter(blueSlider)
alpha = p.readUserDebugParameter(alphaSlider)
p.changeVisualShape(sphereUid, -1, rgbaColor=[red, green, blue, alpha])
p.getCameraImage(320,
200,
flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
time.sleep(0.01)
for i in range(njoint):
p.setJointMotorControlMultiDof(humanoid_id,
i,
p.POSITION_CONTROL, [0, 0, 0, 1],
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
joint_list = ['base', 'Rhip', 'Rknee', 'Rankle', 'Lhip', 'Lknee', 'Lankle']
spherical_ind = [1, 3, 4, 6]
revolute_ind = [2, 5]
target_orn = [[] for _ in range(NLINK)]
while (p.isConnected()):
target_frame = p.readUserDebugParameter(frame_id)
cur_frame = int(np.minimum(target_frame, nframe - 1))
next_frame = int(np.minimum(cur_frame + 1, nframe - 1))
frac = target_frame - cur_frame
cur_basePos = np.array(sample_data['pos']['base'][cur_frame])
next_basePos = np.array(sample_data['pos']['base'][next_frame])
target_basePos = cur_basePos + frac * (next_basePos - cur_basePos)
for i in range(len(joint_list)):
cur_orn = sample_data['orn'][joint_list[i]][cur_frame]
next_orn = sample_data['orn'][joint_list[i]][next_frame]
if i in JOINT_SPHERICAL:
target_orn[i] = p.getQuaternionSlerp(cur_orn, next_orn, frac)
elif i in JOINT_REVOLUTE:
target_orn[i] = cur_orn + frac * (next_orn - cur_orn)
p.loadSDF("stadium.sdf")
obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
human = obUids[0]
for i in range (p.getNumJoints(human)):
p.setJointMotorControl2(human,i,p.POSITION_CONTROL,targetPosition=0,force=500)
kneeAngleTargetId = p.addUserDebugParameter("kneeAngle",-4,4,-1)
maxForceId = p.addUserDebugParameter("maxForce",0,500,10)
kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL",-4,4,-1)
maxForceLeftId = p.addUserDebugParameter("maxForceL",0,500,10)
kneeJointIndex=11
kneeJointIndexLeft=18
while (1):
time.sleep(0.01)
kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(human,kneeJointIndex,p.POSITION_CONTROL,targetPosition=kneeAngleTarget,force=maxForce)
kneeAngleTargetLeft = p.readUserDebugParameter(kneeAngleTargetLeftId)
maxForceLeft = p.readUserDebugParameter(maxForceLeftId)
p.setJointMotorControl2(human,kneeJointIndexLeft,p.POSITION_CONTROL,targetPosition=kneeAngleTargetLeft,force=maxForceLeft)
if (useRealTime==0):
p.stepSimulation()
'''
wheels = [3, 4, 5, 6]
wheels = [3, 5]
# wheels = [3, 4]
right_side_wheels = [4, 6]
# steering = [3]
# steering = []
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -50, 50, 0)
maxForceSlider = p.addUserDebugParameter("maxForce", 0, 50, 20)
steeringLeftSlider = p.addUserDebugParameter("steeringleft", -1, 1, 0)
steeringRightSlider = p.addUserDebugParameter("steeringright", -1, 1, 0)
t = 0
value_to_add = 0.0
while (True):
maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringLeftVel = p.readUserDebugParameter(steeringLeftSlider)
steeringRightVel = p.readUserDebugParameter(steeringRightSlider)
# print(targetVelocity)
for wheel in wheels:
p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=targetVelocity,
force=maxForce)
for wheel in right_side_wheels:
p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=-targetVelocity,
force=maxForce)
# for steer in steering:
# p.setJointMotorControl2(car, steer, p.POSITION_CONTROL, targetPosition=-steeringAngle)
force1Diff = 0.
force2Diff = 0.
force3Diff = 0.
force4Diff = 0.
# simulation loop
while True:
# read out distance sensors of quadrocopter
sensor_data = read_sensors(copter)
# reset camera position
if useGUI:
pos, orn = p.getBasePositionAndOrientation(copter)
cameraTargetPosition = pos
cameraPitch = p.readUserDebugParameter(cameraPitchSlider)
cameraDistance = p.readUserDebugParameter(cameraDistanceSlider)
cameraYaw = p.readUserDebugParameter(cameraYawSlider)
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw,
cameraPitch, cameraTargetPosition)
# read user input to steer quadrocopter
keys = p.getKeyboardEvents()
for k, v in keys.items():
if k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED):
force1Diff = 5
if k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED):
force1Diff = 0.
if k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED):
force2Diff = 5
#while True:
#gravX = p.readUserDebugParameter(gravXid)
#gravY = p.readUserDebugParameter(gravYid)
#gravZ = p.readUserDebugParameter(gravZid)
#p.setGravity(gravX, gravY, gravZ)
#time.sleep(0.01)
p.setRealTimeSimulation(0)
p.setTimeStep(0.005)
t0 = time.clock()
n_steps = 300
print 'Simulation started.'
with open('/tmp/timestep.dat', 'w') as fp:
while True:
ts0 = time.clock()
gravX = p.readUserDebugParameter(gravXid)
gravY = p.readUserDebugParameter(gravYid)
gravZ = p.readUserDebugParameter(gravZid)
p.setGravity(gravX, gravY, gravZ)
#time.sleep(0.01)
ts1 = time.clock()
p.stepSimulation()
ts2 = time.clock()
dts0, dts1 = ts1 - ts0, ts2 - ts1
#print 'Time step:',dts0,dts1
fp.write('{0} {1} {2}\n'.format(time.clock() - t0, dts0, dts1))
t_simulation = time.clock() - t0
t_step = t_simulation / float(n_steps)
total = t_simulation + t_object
p.disconnect()
kdPoleId = p.addUserDebugParameter("kdPole", 0, 300, 100)
maxForcePoleId = p.addUserDebugParameter("maxForcePole", 0, 5000, 1000)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0, 0, -10)
useRealTimeSim = False
p.setRealTimeSimulation(useRealTimeSim)
timeStep = 0.001
while p.isConnected():
#p.getCameraImage(320,200)
timeStep = p.readUserDebugParameter(timeStepId)
p.setTimeStep(timeStep)
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
kpCart = p.readUserDebugParameter(kpCartId)
kdCart = p.readUserDebugParameter(kdCartId)
maxForceCart = p.readUserDebugParameter(maxForceCartId)
desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
kpPole = p.readUserDebugParameter(kpPoleId)
kdPole = p.readUserDebugParameter(kdPoleId)
maxForcePole = p.readUserDebugParameter(maxForcePoleId)
basePos, baseOrn = p.getBasePositionAndOrientation(pole)
robot_file = URDF("urdfs/tutorial2.urdf", force_recompile=True).get_path()
robot = p.loadURDF(robot_file, cubeStartPos, cubeStartOrientation)
for i in range(p.getNumJoints(robot)):
print(p.getJointInfo(robot, i))
leftWheels = [6, 7]
rightWheels = [2, 3]
leftSlider = p.addUserDebugParameter("leftVelocity", -10, 10, 0)
rightSlider = p.addUserDebugParameter("rightVelocity", -10, 10, 0)
forceSlider = p.addUserDebugParameter("maxForce", -10, 10, 0)
for i in range(240 * 10):
leftVelocity = p.readUserDebugParameter(leftSlider)
rightVelocity = p.readUserDebugParameter(rightSlider)
maxForce = p.readUserDebugParameter(forceSlider)
for wheel in leftWheels:
p.setJointMotorControl2(robot,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=leftVelocity,
force=maxForce)
for wheel in rightWheels:
p.setJointMotorControl2(robot,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=rightVelocity,
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 time kpOrg = [0,0,0,0,0,0,0,1000,1000,1000,1000,100,100,100,100,500,500,500,500,500,400,400,400,400,400,400,400,400,300,500,500,500,500,500,400,400,400,400,400,400,400,400,300] kdOrg = [0,0,0,0,0,0,0,100,100,100,100,10,10,10,10,50,50,50,50,50,40,40,40,40,40,40,40,40,30,50,50,50,50,50,40,40,40,40,40,40,40,40,30] once=True p.getCameraImage(320,200) while (p.isConnected()): if useGUI: erp = p.readUserDebugParameter(erpId) kpMotor = p.readUserDebugParameter(kpMotorId) maxForce=p.readUserDebugParameter(forceMotorId) frameReal = p.readUserDebugParameter(frameId) else: erp = 0.2 kpMotor = 0.2 maxForce=1000 frameReal = 0 kp=kpMotor frame = int(frameReal) frameNext = frame+1 if (frameNext >= numFrames): frameNext = frame
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)
if (cid < 0):
cid = p.connect(p.GUI)
restitutionId = p.addUserDebugParameter("restitution", 0, 1, 1)
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=")
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True)
rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0)
pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0)
yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0)
fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0)
fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0)
fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0)
while True:
roll = p.readUserDebugParameter(rollId)
pitch = p.readUserDebugParameter(pitchId)
yaw = p.readUserDebugParameter(yawId)
x = p.readUserDebugParameter(fwdxId)
y = p.readUserDebugParameter(fwdyId)
z = p.readUserDebugParameter(fwdzId)
orn = p.getQuaternionFromEuler([roll, pitch, yaw])
p.resetBasePositionAndOrientation(q, [x, y, z], orn)
#p.stepSimulation()#not really necessary for this demo, no physics used
c_lf = p.createConstraint(vision,knee_front_leftL_joint,vision,motor_front_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,maxForce=maxGearForce) c_rb = p.createConstraint(vision,knee_back_rightR_joint,vision,motor_back_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,maxForce=maxGearForce) c_lb = p.createConstraint(vision,knee_back_leftL_joint,vision,motor_back_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,maxForce=maxGearForce) p.setRealTimeSimulation(1) for i in range (1): #while (1): motA_rf = p.readUserDebugParameter(motA_rf_Id) motB_rf = p.readUserDebugParameter(motB_rf_Id) motC_rf = p.readUserDebugParameter(motC_rf_Id) erp_rf = p.readUserDebugParameter(erp_rf_Id) relPosTarget_rf = p.readUserDebugParameter(relPosTarget_rf_Id) #motC_rf p.setJointMotorControl2(vision,motor_front_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rf,force=maxMotorForce) p.setJointMotorControl2(vision,motor_front_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rf,force=maxMotorForce) p.setJointMotorControl2(vision,motor_front_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rf,force=maxMotorForce) p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,erp=erp_rf, relativePositionTarget=relPosTarget_rf,maxForce=maxGearForce) motA_lf = p.readUserDebugParameter(motA_lf_Id) motB_lf = p.readUserDebugParameter(motB_lf_Id) motC_lf = p.readUserDebugParameter(motC_lf_Id) erp_lf = p.readUserDebugParameter(erp_lf_Id) relPosTarget_lf = p.readUserDebugParameter(relPosTarget_lf_Id)
controller._P_ft[2, 2])
i_slider = pb.addUserDebugParameter('i_f', 0.0, 100.,
controller._I_ft[2, 2])
w_slider = pb.addUserDebugParameter('windup', 0.0, 100.,
controller._windup_guard[2, 0])
i = 0
while i < y_traj.size:
now = time.time()
ee_pos, _ = world.robot.ee_pose()
wrench = world.robot.get_ee_wrench(local=False)
# print wrench
goal_pos[1] = y_traj[i]
controller._P_ft[2, 2] = pb.readUserDebugParameter(p_slider)
controller._I_ft[2, 2] = pb.readUserDebugParameter(i_slider)
controller._windup_guard[2,
0] = pb.readUserDebugParameter(w_slider)
controller.update_goal(goal_pos, goal_ori,
np.asarray([0., 0., target_force]))
fx_deque.append(wrench[0])
fy_deque.append(wrench[1])
fz_deque.append(wrench[2])
elapsed = time.time() - now
sleep_time = (1. / slow_rate) - elapsed
if sleep_time > 0.0:
time.sleep(sleep_time)
set_initial_config(robot, joint_id)
pybullet_util.set_link_damping(robot, link_id.values(), 0., 0.)
pybullet_util.set_joint_friction(robot, joint_id, 0)
nominal_sensor_data = pybullet_util.get_sensor_data(
robot, joint_id, link_id, pos_basejoint_to_basecom,
rot_basejoint_to_basecom)
print("initial joint_pos: ", nominal_sensor_data['joint_pos'])
p.setRealTimeSimulation(1)
paramIds = []
for k in joint_id.keys():
paramIds.append(
p.addUserDebugParameter(k, -4, 4,
nominal_sensor_data['joint_pos'][k]))
while (1):
p.setGravity(0, 0, -9.81)
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(robot,
list(joint_id.values())[i],
p.POSITION_CONTROL,
targetPos,
force=5 * 240.)
time.sleep(0.01)
def set_camera(self):
dist = p.readUserDebugParameter(self.dist)
yaw = p.readUserDebugParameter(self.yaw)
pitch = p.readUserDebugParameter(self.pitch)
p.resetDebugVisualizerCamera(dist, yaw, pitch, self.target_pos)
return
obstaclePos = math.cos(alpha)*r, \
math.sin(alpha)*r
p.resetBasePositionAndOrientation(
obstacle[0], [obstaclePos[0], obstaclePos[1], 0.03],
cubeStartOrientation)
obstaclesPos += [obstaclePos]
# Mise à jour de la position cible
if not autoGoal:
if autoMovingGoal:
goalPos = np.array(
[math.cos(t * 0.1) * 1.5,
math.sin(t * 0.1) * 1.5])
else:
goalPos = np.array([
p.readUserDebugParameter(goalX),
p.readUserDebugParameter(goalY)
])
else:
goalPos = newGoal
p.resetBasePositionAndOrientation(goal, [goalPos[0], goalPos[1], 0.03],
cubeStartOrientation)
pos = p.getLinkState(holo, 1)[0]
orientation = p.getEulerFromQuaternion(p.getLinkState(holo, 0)[1])
robotYaw = orientation[2]
robotPos = np.array([pos[0], pos[1]])
# Test si on a atteint la position cible
dist = np.linalg.norm(robotPos - goalPos)
def test_env(env, arg_dict):
#arg_dict["gui"] == 1
debug_mode = True
spawn_objects = False
action_control = "keyboard" #"observation", "random", "keyboard" or "slider"
visualize_sampling = False
visualize_traj = True
env.render("human")
#env.reset()
joints = [
'Joint1', 'Joint2', 'Joint3', 'Joint4', 'Joint5', 'Joint6', 'Joint7',
'Joint 8', 'Joint 9', 'Joint10', 'Joint11', 'Joint12', 'Joint13',
'Joint14', 'Joint15', 'Joint16', 'Joint17', 'Joint 18', 'Joint 19'
]
jointparams = [
'Jnt1', 'Jnt2', 'Jnt3', 'Jnt4', 'Jnt5', 'Jnt6', 'Jnt7', 'Jnt 8',
'Jnt 9', 'Jnt10', 'Jnt11', 'Jnt12', 'Jnt13', 'Jnt14', 'Jnt15', 'Jnt16',
'Jnt17', 'Jnt 18', 'Jnt 19'
]
cube = [
'Cube1', 'Cube2', 'Cube3', 'Cube4', 'Cube5', 'Cube6', 'Cube7', 'Cube8',
'Cube9', 'Cube10', 'Cube11', 'Cube12', 'Cube13', 'Cube14', 'Cube15',
'Cube16', 'Cube17', 'Cube18', 'Cube19'
]
cubecount = 0
if debug_mode:
if arg_dict["gui"] == 0:
print("Add --gui 1 parameter to visualize environment")
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(1.7, 200, -20, [-0.1, .0, 0.05])
p.setAdditionalSearchPath(pybullet_data.getDataPath())
#newobject = p.loadURDF("cube.urdf", [3.1,3.7,0.1])
#p.changeDynamics(newobject, -1, lateralFriction=1.00)
#p.setRealTimeSimulation(1)
if action_control == "slider":
if "joints" in arg_dict["robot_action"]:
if 'gripper' in arg_dict["robot_action"]:
print("gripper is present")
for i in range(env.action_space.shape[0]):
if i < (env.action_space.shape[0] -
len(env.env.robot.gjoints_rest_poses)):
joints[i] = p.addUserDebugParameter(
joints[i], env.action_space.low[i],
env.action_space.high[i],
env.env.robot.init_joint_poses[i])
else:
joints[i] = p.addUserDebugParameter(
joints[i], env.action_space.low[i],
env.action_space.high[i], .02)
else:
for i in range(env.action_space.shape[0]):
joints[i] = p.addUserDebugParameter(
joints[i], env.action_space.low[i],
env.action_space.high[i],
env.env.robot.init_joint_poses[i])
elif "absolute" in arg_dict["robot_action"]:
if 'gripper' in arg_dict["robot_action"]:
print("gripper is present")
for i in range(env.action_space.shape[0]):
if i < (env.action_space.shape[0] -
len(env.env.robot.gjoints_rest_poses)):
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, arg_dict["robot_init"][i])
else:
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, .02)
else:
for i in range(env.action_space.shape[0]):
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, arg_dict["robot_init"][i])
elif "step" in arg_dict["robot_action"]:
if 'gripper' in arg_dict["robot_action"]:
print("gripper is present")
for i in range(env.action_space.shape[0]):
if i < (env.action_space.shape[0] -
len(env.env.robot.gjoints_rest_poses)):
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, 0)
else:
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, .02)
else:
for i in range(env.action_space.shape[0]):
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, 0)
#maxvelo = p.addUserDebugParameter("Max Velocity", 0.1, 50, env.env.robot.joints_max_velo[0])
#maxforce = p.addUserDebugParameter("Max Force", 0.1, 300, env.env.robot.joints_max_force[0])
lfriction = p.addUserDebugParameter("Lateral Friction", 0, 100, 0)
rfriction = p.addUserDebugParameter("Spinning Friction", 0, 100, 0)
ldamping = p.addUserDebugParameter("Linear Damping", 0, 100, 0)
adamping = p.addUserDebugParameter("Angular Damping", 0, 100, 0)
#action.append(jointparams[i])
if visualize_sampling:
visualize_sampling_area(arg_dict)
#visualgr = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=.005, rgbaColor=[0,0,1,.1])
if action_control == "random":
action = env.action_space.sample()
if action_control == "keyboard":
action = arg_dict["robot_init"]
if "gripper" in arg_dict["robot_action"]:
action.append(.1)
action.append(.1)
if action_control == "slider":
action = []
for i in range(env.action_space.shape[0]):
jointparams[i] = p.readUserDebugParameter(joints[i])
action.append(jointparams[i])
for e in range(10000):
env.reset()
#if spawn_objects:
# cube[e] = p.loadURDF(pkg_resources.resource_filename("myGym", os.path.join("envs", "objects/assembly/urdf/cube_holes.urdf")), [0, 0.5, .1])
#if visualize_traj:
# visualize_goal(info)
for t in range(arg_dict["max_episode_steps"]):
if action_control == "slider":
action = []
for i in range(env.action_space.shape[0]):
jointparams[i] = p.readUserDebugParameter(joints[i])
action.append(jointparams[i])
#env.env.robot.joints_max_velo[i] = p.readUserDebugParameter(maxvelo)
#env.env.robot.joints_max_force[i] = p.readUserDebugParameter(maxforce)
if action_control == "observation":
if arg_dict["robot_action"] == "joints":
action = info['o']["additional_obs"]["joints_angles"] #n
else:
action = info['o']["additional_obs"]["endeff_xyz"]
#action[0] +=.3
elif action_control == "keyboard":
keypress = p.getKeyboardEvents()
#print(action)
if 97 in keypress.keys() and keypress[97] == 1:
action[2] += .03
print(action)
if 122 in keypress.keys() and keypress[122] == 1:
action[2] -= .03
print(action)
if 65297 in keypress.keys() and keypress[65297] == 1:
action[1] -= .03
print(action)
if 65298 in keypress.keys() and keypress[65298] == 1:
action[1] += .03
print(action)
if 65295 in keypress.keys() and keypress[65295] == 1:
action[0] += .03
print(action)
if 65296 in keypress.keys() and keypress[65296] == 1:
action[0] -= .03
print(action)
if 120 in keypress.keys() and keypress[120] == 1:
action[3] -= .03
action[4] -= .03
print(action)
if 99 in keypress.keys() and keypress[99] == 1:
action[3] += .03
action[4] += .03
print(action)
if 100 in keypress.keys() and keypress[100] == 1:
cube[cubecount] = p.loadURDF(
pkg_resources.resource_filename(
"myGym",
os.path.join(
"envs",
"objects/assembly/urdf/cube_holes.urdf")),
[action[0], action[1], action[2] - 0.2])
change_dynamics(cube[cubecount], lfriction, rfriction,
ldamping, adamping)
cubecount += 1
if "step" in arg_dict["robot_action"]:
action[:3] = np.multiply(action[:3], 10)
#for i in range (env.action_space.shape[0]):
# env.env.robot.joints_max_velo[i] = p.readUserDebugParameter(maxvelo)
# env.env.robot.joints_max_force[i] = p.readUserDebugParameter(maxforce)
elif action_control == "random":
action = env.action_space.sample()
#print (f"Action:{action}")
observation, reward, done, info = env.step(action)
if visualize_traj:
#visualize_trajectories(info,action)
p.addUserDebugText(
f"Action (Gripper):{matrix(np.around(np.array(action),5))}",
[.8, .5, 0.2],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[1, 0, 0])
p.addUserDebugText(
f"Endeff:{matrix(np.around(np.array(info['o']['additional_obs']['endeff_xyz']),5))}",
[.8, .5, 0.1],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.0, 1, 0.0])
#p.addUserDebugText(f"Object:{matrix(np.around(np.array(info['o']['actual_state']),5))}",
# [.8, .5, 0.15], textSize=1.0, lifeTime=0.5, textColorRGB=[0.0, 0.0, 1])
p.addUserDebugText(f"Network:{env.env.reward.current_network}",
[.8, .5, 0.25],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.0, 0.0, 1])
p.addUserDebugText(f"Subtask:{env.env.task.current_task}",
[.8, .5, 0.35],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.4, 0.2, 1])
p.addUserDebugText(f"Episode:{e}", [.8, .5, 0.45],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.4, 0.2, .3])
p.addUserDebugText(f"Step:{t}", [.8, .5, 0.55],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.2, 0.8, 1])
#visualize_goal(info)
#if debug_mode:
#print("Reward is {}, observation is {}".format(reward, observation))
#if t>=1:
#action = matrix(np.around(np.array(action),5))
#oaction = env.env.robot.get_joints_states()
#oaction = matrix(np.around(np.array(oaction[0:action.shape[0]]),5))
#diff = matrix(np.around(np.array(action-oaction),5))
#print(env.env.robot.get_joints_states())
#print(f"Step:{t}")
#print (f"RAction:{action}")
#print(f"OAction:{oaction}")
#print(f"DAction:{diff}")
#p.addUserDebugText(f"DAction:{diff}",
# [1, 1, 0.1], textSize=1.0, lifeTime=0.05, textColorRGB=[0.6, 0.0, 0.6])
#time.sleep(.5)
#clear()
#if action_control == "slider":
# action=[]
if "step" in arg_dict["robot_action"]:
action[:3] = [0, 0, 0]
if arg_dict["visualize"]:
visualizations = [[], []]
env.render("human")
for camera_id in range(len(env.cameras)):
camera_render = env.render(mode="rgb_array",
camera_id=camera_id)
image = cv2.cvtColor(camera_render[camera_id]["image"],
cv2.COLOR_RGB2BGR)
depth = camera_render[camera_id]["depth"]
image = cv2.copyMakeBorder(image,
30,
10,
10,
20,
cv2.BORDER_CONSTANT,
value=[255, 255, 255])
cv2.putText(image, 'Camera {}'.format(camera_id), (10, 20),
cv2.FONT_HERSHEY_SIMPLEX, .5, (0, 0, 0), 1, 0)
visualizations[0].append(image)
depth = cv2.copyMakeBorder(depth,
30,
10,
10,
20,
cv2.BORDER_CONSTANT,
value=[255, 255, 255])
cv2.putText(depth, 'Camera {}'.format(camera_id), (10, 20),
cv2.FONT_HERSHEY_SIMPLEX, .5, (0, 0, 0), 1, 0)
visualizations[1].append(depth)
if len(visualizations[0]) % 2 != 0:
visualizations[0].append(
255 *
np.ones(visualizations[0][0].shape, dtype=np.uint8))
visualizations[1].append(
255 *
np.ones(visualizations[1][0].shape, dtype=np.float32))
fig_rgb = np.vstack((np.hstack((visualizations[0][0::2])),
np.hstack((visualizations[0][1::2]))))
fig_depth = np.vstack((np.hstack((visualizations[1][0::2])),
np.hstack((visualizations[1][1::2]))))
cv2.imshow('Camera RGB renders', fig_rgb)
cv2.imshow('Camera depthrenders', fig_depth)
cv2.waitKey(1)
if done:
print("Episode finished after {} timesteps".format(t + 1))
break
#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)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints=currentline[2:14]
#print("joints=",joints)
for j in range (12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped,-1, lower_leg)
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)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0,0,0)
useRealTimeSim = True
p.setRealTimeSimulation(useRealTimeSim)
p.setTimeStep(0.001)
while p.isConnected():
if (pd>=0):
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
kpCart = p.readUserDebugParameter(kpCartId)
kdCart = p.readUserDebugParameter(kdCartId)
maxForceCart = p.readUserDebugParameter(maxForceCartId)
link = 0
p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosCart, desiredVelCart, kdCart, kpCart, maxForceCart])
desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
kpPole = p.readUserDebugParameter(kpPoleId)
kdPole = p.readUserDebugParameter(kdPoleId)
maxForcePole = p.readUserDebugParameter(maxForcePoleId)
link = 1
p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosPole, desiredVelPole, kdPole, kpPole, maxForcePole])
error = self.calculate_error(joints)
if error <= 0.04:
i = i + 6
if i > len(waypoints) - 6: return joints_list[0:num_waypoints]
joints = waypoints[i:i + 6]
if not use_joint_angles:
joints = self.calculate_ik(joints[0:3], joints[-3:])
print("Waypoint {}: {}".format(i / 6, joints))
joints_list.extend(joints)
self.step_joints(joints)
if __name__ == "__main__":
sim = Simulation()
sim.step_joints(
[0, -math.pi / 2, -math.pi / 2, -math.pi / 2, -math.pi / 2, 0])
time.sleep(4)
while True:
x = pybullet.readUserDebugParameter(sim.sliders[0])
y = pybullet.readUserDebugParameter(sim.sliders[1])
z = pybullet.readUserDebugParameter(sim.sliders[2])
Rx = pybullet.readUserDebugParameter(sim.sliders[3])
Ry = pybullet.readUserDebugParameter(sim.sliders[4])
Rz = pybullet.readUserDebugParameter(sim.sliders[5])
position = [x, y, z]
orientation = [Rx, Ry, Rz]
joint_angles = sim.calculate_ik(position, orientation)
sim.step_joints(joint_angles)
def read_param(self, name):
uid = self.debug_names[name]
value = p.readUserDebugParameter(itemUniqueId=uid,
physicsClientId=self.sim.id)
return value
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
steering = [0, 2]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -50, 50, 0)
maxForceSlider = p.addUserDebugParameter("maxForce", 0, 50, 20)
steeringSlider = p.addUserDebugParameter("steering", -1, 1, 0)
activeController = -1
while (True):
maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = p.readUserDebugParameter(steeringSlider)
#print(targetVelocity)
events = p.getVREvents()
for e in events:
if (e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED):
activeController = e[CONTROLLER_ID]
if (activeController == e[CONTROLLER_ID]):
orn = e[2]
eul = p.getEulerFromQuaternion(orn)
steeringAngle = eul[0]
targetVelocity = 20.0 * e[3]
for wheel in wheels:
import pybullet
import pybullet_data
pybullet.connect(pybullet.GUI)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
plane = pybullet.loadURDF("plane.urdf")
robot = pybullet.loadURDF(
"../kuka_experimental/kuka_kr210_support/urdf/kr210l150.urdf", [0, 0, 0],
useFixedBase=1) # use a fixed base!
pybullet.setGravity(0, 0, -9.81)
pybullet.setRealTimeSimulation(1) #this makes the simulation real time
pybullet.addUserDebugParameter('x')
while 1:
x = pybullet.readUserDebugParameter(0)
pybullet.setJointMotorControlArray(robot,
range(6),
pybullet.POSITION_CONTROL,
targetPositions=[x] * 6)
c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000)
c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000)
steering = [0,2]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-50,50,0)
maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20)
steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
while (True):
maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = p.readUserDebugParameter(steeringSlider)
#print(targetVelocity)
for wheel in wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce)
for steer in steering:
p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=-steeringAngle)
steering
if (useRealTimeSim==0):
p.stepSimulation()
time.sleep(0.01)
button_uid = p.loadURDF("/urdf/simple_button.urdf", [0, 0, 0])
glider_idx = 1
button_link_idx = 1
p.setGravity(0, 0, -10)
t = 0
useSimulation = True
useRealTimeSimulation = False
p.setRealTimeSimulation(useRealTimeSimulation)
button_pos_slider = p.addUserDebugParameter("button_pos", 0, 1, 0.1)
while True:
if useRealTimeSimulation:
dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
else:
t = t + 0.1
if useSimulation and not useRealTimeSimulation:
p.stepSimulation()
button_position = p.readUserDebugParameter(button_pos_slider)
# p.applyExternalForce(button_uid, button_link_idx, (0, 0, 10), (0, 0, 0), p.LINK_FRAME)
p.setJointMotorControl2(button_uid,
glider_idx,
controlMode=p.POSITION_CONTROL,
targetPosition=button_position)
sliders["segment_y1"] = p.addUserDebugParameter("segment_y1", -1, 1, 0.0)
sliders["segment_z1"] = p.addUserDebugParameter("segment_z1", -1, 1, 0.0)
sliders["segment_x2"] = p.addUserDebugParameter("segment_x2", -1, 1, 0.4)
sliders["segment_y2"] = p.addUserDebugParameter("segment_y2", -1, 1, 0.2)
sliders["segment_z2"] = p.addUserDebugParameter("segment_z2", -1, 1, 0.2)
sliders["segment_duration"] = p.addUserDebugParameter(
"segment_duration", 0.01, 10, 3)
lastLine = time.time()
lastInverse = 0
targets = {"motor" + str(k + 1): 0 for k in range(3)}
while True:
if sim.t > 1.0:
if args.mode == "direct":
for joint in joints:
targets[joint] = p.readUserDebugParameter(sliders[joint])
T = kinematicsnew.computeDK(-targets["motor1"], -targets["motor2"],
targets["motor3"])
# T = model.direct(targets)
T[0] += bx
T[2] += bz
p.resetBasePositionAndOrientation(target, T, [0, 0, 0, 1])
elif args.mode == "inverse" or args.mode == "inverse-iterative":
x = p.readUserDebugParameter(sliders["target_x"])
y = p.readUserDebugParameter(sliders["target_y"])
z = p.readUserDebugParameter(sliders["target_z"])
p.resetBasePositionAndOrientation(target, [x + bx, y, z + bz],
[0, 0, 0, 1])
def step(self):
quadruped = self.quadruped
bodyPos, bodyOrn = p.getBasePositionAndOrientation(quadruped)
linearVel, angularVel = p.getBaseVelocity(quadruped)
bodyEuler = p.getEulerFromQuaternion(bodyOrn)
kp = p.readUserDebugParameter(self.IDkp)
kd = p.readUserDebugParameter(self.IDkd)
maxForce = p.readUserDebugParameter(self.IDmaxForce)
self.handleCamera(bodyPos, bodyOrn)
self.addInfoText(bodyPos, bodyEuler, linearVel, angularVel)
if self.checkSimulationReset(bodyOrn):
return False
# Calculate Angles with the input of FeetPos,BodyRotation and BodyPosition
angles = self.kin.calcIK(self.Lp, self.rot, self.pos)
for lx, leg in enumerate(
['front_left', 'front_right', 'rear_left', 'rear_right']):
for px, part in enumerate(['shoulder', 'leg', 'foot']):
j = self.jointNameToId[leg + "_" + part]
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=j,
controlMode=p.POSITION_CONTROL,
targetPosition=angles[lx][px] *
self.dirs[lx][px],
positionGain=kp,
velocityGain=kd,
force=maxForce)
nowLidarTime = time.time()
if (nowLidarTime - self.lastLidarTime > .2):
numThreads = 0
results = p.rayTestBatch(self.rayFrom,
self.rayTo,
numThreads,
parentObjectUniqueId=quadruped,
parentLinkIndex=0)
for i in range(self.numRays):
hitObjectUid = results[i][0]
hitFraction = results[i][2]
hitPosition = results[i][3]
if (hitFraction == 1.):
if self.debugLidar:
p.addUserDebugLine(self.rayFrom[i],
self.rayTo[i],
self.rayMissColor,
replaceItemUniqueId=self.rayIds[i],
parentObjectUniqueId=quadruped,
parentLinkIndex=0)
else:
localHitTo = [
self.rayFrom[i][0] + hitFraction *
(self.rayTo[i][0] - self.rayFrom[i][0]),
self.rayFrom[i][1] + hitFraction *
(self.rayTo[i][1] - self.rayFrom[i][1]),
self.rayFrom[i][2] + hitFraction *
(self.rayTo[i][2] - self.rayFrom[i][2])
]
dis = math.sqrt(localHitTo[0]**2 + localHitTo[1]**2 +
localHitTo[2]**2)
if self.debugLidar:
p.addUserDebugLine(self.rayFrom[i],
localHitTo,
self.rayHitColor,
replaceItemUniqueId=self.rayIds[i],
parentObjectUniqueId=quadruped,
parentLinkIndex=0)
lastLidarTime = nowLidarTime
# let the Simulator do the rest
if (self.useRealTime):
self.t = time.time() - self.ref_time
else:
self.t = self.t + self.fixedTimeStep
if (self.useRealTime == False):
p.stepSimulation()
time.sleep(self.fixedTimeStep)
mimicParentID = jointID
print("There are {} mimic child".format(len(mimicChildList)))
# contraints to simulate mimic tag in robotiq_c2.urdf
for i, mimicChildID in enumerate(mimicChildList):
c = p.createConstraint(robotID,
mimicParentID,
robotID,
mimicChildID,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=mimicMul[i], maxForce=10000)
constraintInfo = p.getConstraintInfo(c)
# start simulation
try:
flag = True
gripper_control = p.addUserDebugParameter("gripper", 0, 0.343, 0)
while (flag):
jointPose = p.readUserDebugParameter(gripper_control)
p.setJointMotorControl2(robotID,
mimicParentID,
p.POSITION_CONTROL,
targetPosition=jointPose)
p.stepSimulation()
p.disconnect()
except:
p.disconnect()
rightKnee=10
rightAnkle=11
leftHip = 12
leftKnee=13
leftAnkle=14
import time
once=True
p.getCameraImage(320,200)
maxForce=1000
while (p.isConnected()):
frameReal = p.readUserDebugParameter(frameId)
frame = int(frameReal)
frameNext = frame+1
if (frameNext >= numFrames):
frameNext = frame
frameFraction = frameReal - frame
#print("frameFraction=",frameFraction)
#print("frame=",frame)
#print("frameNext=", frameNext)
#getQuaternionSlerp
frameData = motion_dict['Frames'][frame]
frameDataNext = motion_dict['Frames'][frameNext]
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])
def act(self, obs, stochastic=False):
return [
pybullet.readUserDebugParameter(slider) for slider in self._sliders
]
jointDict["Front"]["vecParaId"] = p.addUserDebugParameter(
"FrontWheel_veclocity", -500, 500, 100)
jointDict["Bar"]["positionID"] = p.addUserDebugParameter(
"Bar_position", -5, 5, 0)
############
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)
p.setRealTimeSimulation(1)
while (1):
p.setGravity(0, 0, p.readUserDebugParameter(gravId))
bar_Pos = p.readUserDebugParameter(jointDict["Bar"]["positionID"])
p.setJointMotorControl2(humanoid,
jointDict["Bar"]["id"],
p.POSITION_CONTROL,
bar_Pos,
force=5 * 240.)
back_vec = p.readUserDebugParameter(jointDict["Back"]["vecParaId"])
p.setJointMotorControl2(humanoid,
jointDict["Back"]["id"],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=back_vec,
force=maxForce)
targetPosition=shoulderRoll,
force=1000)
# this can be anywhere in the file
p.setGravity(0, 0, -9.8)
# set the simulator frequency to 240Hz. In practice that's a lot. I'd go higher than 50Hz (stability) and probably around 100Hz (good accuracy-speed tradeoff)
timeStep = 1. / 240.
p.setTimeStep(timeStep)
# add motors that you wanna actuate to this list. You find them by looking at the terminal output from line 38-39
motors = [1, 2, 13, 14, 15, 16, 17, 18, 26, 27, 28, 29, 30, 31]
debugParams = []
# motors = [3, 4, 6, 8, 10, 14]
# make it so that for each one of the motors we add a slider to we can test each motor
for i in range(len(motors)):
motor = p.addUserDebugParameter("motor{}".format(i + 1), -1, 1, 0)
debugParams.append(motor)
# in this loop, we query each user debug slider and set the motor to that value. In practice, these motor signals would come from your policy
while (1):
for j in range(len(motors)):
pos = (math.pi / 2) * p.readUserDebugParameter(debugParams[j])
p.setJointMotorControl2(nao,
motors[j],
p.POSITION_CONTROL,
targetPosition=pos)
p.stepSimulation()
time.sleep(timeStep)
robot = obUids[1]
gravId = p.addUserDebugParameter("gravity", -10, 10, gravity)
jointIds = []
paramIds = []
p.setPhysicsEngineParameter(numSolverIterations=10)
p.changeDynamics(robot, -1, linearDamping=0, angularDamping=0)
# create the interface that enables the user to modify gravity and the positions of the jonts
for j in range(p.getNumJoints(robot)):
p.changeDynamics(robot, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(robot, 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))
# run the simulation and set the desired position of the joints set by the user
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(robot, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
time.sleep(0.01)
p.setRealTimeSimulation(useRealTimeSim)
numJoints = p.getNumJoints(pole)
desired_q = np.array([2, 0])
desired_qdot = np.array([0, 0])
kpCart = 500
kdCart = 150
kpPole = 10
kdPole = 1
prev_q = 0
kps = [kpCart, kpPole]
kds = [kdCart, kdPole]
while p.isConnected():
timeStep = p.readUserDebugParameter(timeStepId)
p.setTimeStep(timeStep)
jointStates = p.getJointStates(pole, [0, 1])
q = np.array([jointStates[0][0], jointStates[1][0]])
qdot = np.array((q - prev_q) / timeStep)
qError = desired_q - q
qdotError = desired_qdot - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
forces = Kp.dot(qError) + Kd.dot(qdotError)
maxF = 1000
force1 = np.clip(forces[0], -maxF, maxF)
maxF = 1000
force2 = np.clip(forces[1], -maxF, maxF)
forces = [force1, force2]
# Get depth values using the OpenGL renderer
projection_matrix = p.computeProjectionMatrixFOV(
fov, aspect, near, far)
images = p.getCameraImage(width,
height,
view_matrix,
projection_matrix,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255.
#plt.imshow(rgb_opengl)
#plt.title('RGB image')
#plt.pause(0.0001)
# read the value of task parameter
x = p.readUserDebugParameter(xin)
y = p.readUserDebugParameter(yin)
z = p.readUserDebugParameter(zin)
roll = p.readUserDebugParameter(rollId)
pitch = p.readUserDebugParameter(pitchId)
yaw = p.readUserDebugParameter(yawId)
orn = p.getQuaternionFromEuler([roll, pitch, yaw])
# read the value of camera parameter
cc1 = p.readUserDebugParameter(c1)
cc2 = p.readUserDebugParameter(c2)
cc3 = p.readUserDebugParameter(c3)
cc4 = p.readUserDebugParameter(c4)
cc5 = p.readUserDebugParameter(c5)
cc6 = p.readUserDebugParameter(c6)
view_matrix = p.computeViewMatrix([cc1, cc2, cc3], [cc4, cc5, cc6],
# info = p.getJointInfo(car, joint_number)
# # print(info)
# print(info[0], ": ", info[1])
import pybullet as p
from time import sleep
wheel_indices = [1, 3, 4, 5]
hinge_indices = [0, 2]
p.connect(p.GUI)
p.setGravity(0, 0, -10)
angle = p.addUserDebugParameter('Steering', -0.5, 0.5, 0)
throttle = p.addUserDebugParameter('Throttle', -20, 20, 0)
car = p.loadURDF('simplecar.urdf', [0, 0, 0.1])
plane = p.loadURDF('simpleplane.urdf')
while True:
user_angle = p.readUserDebugParameter(angle)
user_throttle = p.readUserDebugParameter(throttle)
for joint_index in wheel_indices:
p.setJointMotorControl2(car,
joint_index,
p.VELOCITY_CONTROL,
targetVelocity=user_throttle)
for joint_index in hinge_indices:
p.setJointMotorControl2(car,
joint_index,
p.POSITION_CONTROL,
targetPosition=user_angle)
p.stepSimulation()
def value(self):
return p.readUserDebugParameter(self.handle_id)
objectNum = p.getNumBodies()
print('record num:'),
print(recordNum)
print('item num:'),
print(itemNum)
def Step(stepIndex):
for objectId in range(objectNum):
record = log[stepIndex*objectNum+objectId]
Id = record[2]
pos = [record[3],record[4],record[5]]
orn = [record[6],record[7],record[8],record[9]]
p.resetBasePositionAndOrientation(Id,pos,orn)
numJoints = p.getNumJoints(Id)
for i in range (numJoints):
jointInfo = p.getJointInfo(Id,i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(Id,i,record[qIndex-7+17])
stepIndexId = p.addUserDebugParameter("stepIndex",0,recordNum/objectNum-1,0)
while True:
stepIndex = int(p.readUserDebugParameter(stepIndexId))
Step(stepIndex)
p.stepSimulation()
Step(stepIndex)
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)
