lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=",
enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5,
enableCollision)
jointIds = []
paramIds = []
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 80)
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)
# print(jointIds)
#time.sleep(20)
p.getCameraImage(1280, 720)
#p.setRealTimeSimulation(0)
p.setGravity(0, 0, -10) # set gravity
plane = p.loadURDF("plane.urdf") # load the plane
bot = p.loadURDF("finalbot.urdf", [0, 0, 1]) # load the bot in
totalJoints = p.getNumJoints(bot) # total bot joints
print('\njoints: {}\n'.format(totalJoints))
for i in range(totalJoints):
print("{}\n".format(p.getJointInfo(bot, i)))
#print('\n\n')
#print(p.getJointInfo(bot, 0))
lwVelocitySlider = p.addUserDebugParameter("LWheelVelocity",-100,100,0)
rwVelocitySlider = p.addUserDebugParameter("RWheelVelocity",-100,100,0)
maxForceRWSlider = p.addUserDebugParameter("maxForceRWs",0,300,15)
turnVelocitySlider = p.addUserDebugParameter("turnVelocity",-30,30,0)
maxTurnForceSlider = p.addUserDebugParameter("maxTurnForce",0,30,15)
fwVelocitySlider = p.addUserDebugParameter("forwardWheelsVelocity",-30,30,0)
maxFWForceSlider = p.addUserDebugParameter("maxFWForce",0,30,15)
p.setRealTimeSimulation(1)
p.changeDynamics(bot, 9, rollingFriction=0)
p.changeDynamics(bot, 8, rollingFriction=0)
redSlider = p.addUserDebugParameter("red",0,1,1)
pole3 = p.loadURDF("cartpole.urdf", [0, 2, 0], useMaximalCoordinates=useMaximalCoordinates)
pole4 = p.loadURDF("cartpole.urdf", [0, 3, 0], useMaximalCoordinates=useMaximalCoordinates)
exPD = PDControllerExplicitMultiDof(p)
sPD = PDControllerStable(p)
for i in range(p.getNumJoints(pole2)):
#disable default constraint-based motors
p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole2, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole3, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole4, i, p.POSITION_CONTROL, targetPosition=0, force=0)
#print("joint",i,"=",p.getJointInfo(pole2,i))
timeStepId = p.addUserDebugParameter("timeStep", 0.001, 0.1, 0.01)
desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0)
kpCartId = p.addUserDebugParameter("kpCart", 0, 500, 1300)
kdCartId = p.addUserDebugParameter("kdCart", 0, 300, 150)
maxForceCartId = p.addUserDebugParameter("maxForceCart", 0, 5000, 1000)
textColor = [1, 1, 1]
shift = 0.05
p.addUserDebugText("explicit PD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole,
parentLinkIndex=1)
p.addUserDebugText("explicit PD plugin", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole2,
physicsClient = p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
planeUid = p.loadURDF(
os.path.join(parentdir,
"My_env_test/neobotix_schunk_pybullet/data/plane.urdf"))
neobotixschunkUid = p.loadURDF(os.path.join(
parentdir,
"My_env_test/neobotix_schunk_pybullet/data/neobotixschunk/mp500lwa4d_test.urdf"
),
useFixedBase=False,
flags=p.URDF_USE_SELF_COLLISION)
joint_observ1 = joint(neobotixschunkUid, 7)
joint_observ2 = joint(neobotixschunkUid, 9)
#
quitId = p.addUserDebugParameter("quit", 0, 1, 0)
#
p.setJointMotorControl2(neobotixschunkUid,
9,
p.POSITION_CONTROL,
targetPosition=0,
force=50) #fz的方向为arm7旋转轴，因此arm6旋转时，会使重力在fz上的分力减小
# p.setJointMotorControl2(neobotixschunkUid,9,p.POSITION_CONTROL,
# targetPosition=0,force=10) #力是表示在child_link坐标系上的，因此随着旋转fx，fy会变化
# p.setJointMotorControl2(neobotixschunkUid,9,p.POSITION_CONTROL,
# targetPosition=0,force=5)
# p.setJointMotorControl2(neobotixschunkUid,8,p.POSITION_CONTROL,
# targetPosition=0,force=5)
# p.setJointMotorControl2(neobotixschunkUid,7,p.POSITION_CONTROL,
# targetPosition=0,force=5)
# p.setJointMotorControl2(neobotixschunkUid,6,p.POSITION_CONTROL,
panda = p.loadURDF("franka_panda/panda.urdf", useFixedBase=True)
p.setGravity(0,0,-9.8)
jointIds = []
paramIds = []
jointPositions=[0,0,0,-1.4,0,1.4,0.71,0.014,0.016]
index = 0
print("numJoints = ",p.getNumJoints(panda))
for j in range(p.getNumJoints(panda)):
p.changeDynamics(panda, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(panda, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC):
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -0.1, 0.1, jointPositions[index]))
index=index+1
if (jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, jointPositions[index]))
index=index+1
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(panda, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
p.stepSimulation()
p.saveWorld("bla")
enableCollision = 1
print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision)
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=[]
#p.loadURDF("plane.urdf")
p.loadSDF(os.path.join(pybullet_data.getDataPath(),"stadium.sdf"))
car = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"racecar/racecar.urdf"))
for i in range (p.getNumJoints(car)):
print (p.getJointInfo(car,i))
inactive_wheels = [3,5,7]
wheels = [2]
for wheel in inactive_wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
steering = [4,6]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-10,10,0)
maxForceSlider = p.addUserDebugParameter("maxForce",0,10,10)
steeringSlider = p.addUserDebugParameter("steering",-0.5,0.5,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
c = p.createConstraint(car,16,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,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
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)
#path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt"
#path = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
#p.loadURDF("plane.urdf",[0,0,-1.03])
print("path = ", path)
with open(path, 'r') as f:
motion_dict = json.load(f)
#print("motion_dict = ", motion_dict)
print("len motion=", len(motion_dict))
print(motion_dict['Loop'])
numFrames = len(motion_dict['Frames'])
print("#frames = ", numFrames)
frameId= p.addUserDebugParameter("frame",0,numFrames-1,0)
erpId = p.addUserDebugParameter("erp",0,1,0.2)
kpMotorId = p.addUserDebugParameter("kpMotor",0,1,.2)
forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000)
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]]
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.25])
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)
#you can set the restitution (bouncyness) of an object in the URDF file
#or using changeDynamics
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
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)
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
time.sleep(sleep_time)
i += 1
else:
print("Never reached force threshold for switching controller")
f_ctrl = False
if f_ctrl:
print("Switching to force control along Z axis")
y_traj = np.linspace(goal_pos[1], goal_pos[1] - 0.2, 400)
controller.change_ft_directions([0, 0, 1, 0, 0, 0])
target_force = -21.
p_slider = pb.addUserDebugParameter('p_f', 0.1, 2.,
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)
print("\tID: {}".format(jointID))
print("\tname: {}".format(jointName))
print("\ttype: {}".format(jointType))
print("\tlower limit: {}".format(jointLowerLimit))
print("\tupper limit: {}".format(jointUpperLimit))
print("------------------------------------------")
# get links
linkIDs = list(map(lambda linkInfo: linkInfo[1],
p.getVisualShapeData(robotID)))
linkNum = len(linkIDs)
# start simulation
try:
flag = True
textPose = list(p.getBasePositionAndOrientation(robotID)[0])
textPose[2] += 1
p.addUserDebugText("Press \'w\' and magic!!", textPose, [255, 0, 0], 1)
prevLinkID = 0
linkIDIn = p.addUserDebugParameter("linkID", 0, linkNum - 1e-3, 0)
while (flag):
p.stepSimulation()
linkID = p.readUserDebugParameter(linkIDIn)
if linkID != prevLinkID:
p.setDebugObjectColor(robotID, int(prevLinkID), [255, 255, 255])
p.setDebugObjectColor(robotID, int(linkID), [255, 0, 0])
prevLinkID = linkID
p.disconnect()
except:
p.disconnect()
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)
# setup sisbot
robot_start_pos = [0,0,0]
robot_start_orn =bullet.getQuaternionFromEuler([0,0,0])
print("----------------------------------------")
print("Loading robot from {}".format(sisbot_model_path))
robot = bullet.loadURDF(sisbot_model_path, robot_start_pos, robot_start_orn, flags=bullet.URDF_USE_INERTIA_FROM_FILE)
plane = bullet.loadURDF(plane_model_path)
joints, control_robotiq_c2, control_joints, mimic_parent_name = setup_sisbot(bullet, robot)
eef_id = 7 # ee_link
# start simulation
ABSE = lambda a,b: abs(a-b)
try:
flag = True
x_in = bullet.addUserDebugParameter("x", -2, 2, 0)
y_in = bullet.addUserDebugParameter("y", -2, 2, 0)
z_in = bullet.addUserDebugParameter("z", 0.5, 2, 1)
i = 0
while flag:
i += 1
print ("i : ", i)
x = bullet.readUserDebugParameter(x_in)
y = bullet.readUserDebugParameter(y_in)
z = bullet.readUserDebugParameter(z_in)
joint_pose = bullet.calculateInverseKinematics(robot, eef_id, [x,y,z])
for i, name in enumerate(control_joints):
joint = joints[name]
pose = joint_pose[i]
rXYZ = bullet.getLinkState(robot, eef_id)[0] # real XYZ
default="direct",
help="Available modes : direct, inverse, circle, triangle",
)
args = parser.parse_args()
sim = simulation.Simulation("rrr/robot.urdf", fixed=True, panels=True)
sliders = {}
target = None
joints = sim.getJoints()
bx = 0.07
bz = 0.25
if args.mode == "direct":
target = p.loadURDF("target2/robot.urdf")
for joint in joints:
sliders[joint] = p.addUserDebugParameter(joint, -math.pi, math.pi, 0.0)
elif args.mode == "inverse" or args.mode == "inverse-iterative":
target = p.loadURDF("target2/robot.urdf")
sliders["target_x"] = p.addUserDebugParameter("target_x", -1, 1, 0.4)
sliders["target_y"] = p.addUserDebugParameter("target_y", -1, 1, 0)
sliders["target_z"] = p.addUserDebugParameter("target_z", -1, 1, 0.25)
elif args.mode == "triangle" or args.mode == "triangle-points":
sliders["triangle_x"] = p.addUserDebugParameter("triangle_x", 0.01, 0.8,
0.4)
sliders["triangle_z"] = p.addUserDebugParameter("triangle_z", -0.2, 0.3, 0)
sliders["triangle_h"] = p.addUserDebugParameter("triangle_h", 0.01, 0.3,
0.1)
sliders["triangle_w"] = p.addUserDebugParameter("triangle_w", 0.01, 0.3,
0.2)
elif args.mode == "circle" or args.mode == "circle-points":
sliders["circle_x"] = p.addUserDebugParameter("circle_x", -1, 1, 0.4)
_ = system('cls')
# for mac and linux(here, os.name is 'posix')
else:
_ = system('clear')
robot = spotmicroai.Robot(True, True, reset)
spurWidth = robot.W / 2 + 20
stepLength = 0
stepHeight = 72
iXf = 120
iXb = -132
IDheight = p.addUserDebugParameter("height", -40, 90, 20)
Lp = np.array([[iXf, -100, spurWidth, 1], [iXf, -100, -spurWidth, 1],
[-50, -100, spurWidth, 1], [-50, -100, -spurWidth, 1]])
resetPose()
trotting = TrottingGait()
def main(id, command_status):
s = False
while True:
bodyPos = robot.getPos()
bodyOrn, _, _ = robot.getIMU()
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)
import pybullet as p
import time
import pybullet_data
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
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
p.setRealTimeSimulation(useRealTime)
p.setGravity(0,0,-10)
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)
camDistance = 4
pixelWidth = 320
pixelHeight = 200
nearPlane = 0.01
farPlane = 100
fov = 60
# plt.ion()
# img = [[1,2,3]*50]*100#np.random.rand(200, 320)
# image = plt.imshow(img,interpolation='none',animated=True,label="blah")
# ax = plt.gca()
#### joint control stuff
if DEBUG:
targetVelocityLeftSlider = p.addUserDebugParameter("wheelVelocityLeft",
-20, 20, 0)
targetVelocityRightSlider = p.addUserDebugParameter(
"wheelVelocityRight", -20, 20, 0)
maxForceSlider = p.addUserDebugParameter("maxForce", 0, 20, 20)
wheels = {
"left": 3,
"right": 2
} # by looking at output of p.getJointInfo() in loop
# also they are inverted in the model (or named badly)
i = 0
frame_time = 0
while True:
start = time.time()
targetPosition=[0],
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
p.setJointMotorControlMultiDof(linkage,
jointInfoList['dogbone_joint_far_right'],
controlMode=p.POSITION_CONTROL,
targetPosition=[0],
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
# Create debug window to control joints
neck_ctrl = p.addUserDebugParameter("neck_joint", -1.57, 1.57, 0)
pitch_piece_ctrl = p.addUserDebugParameter("pitch_piece_joint", -1.57, 1.57, 0)
skull_ctrl = p.addUserDebugParameter("skull_joint", -1.57, 1.57, 0)
linear_motor_far_left_ctrl = p.addUserDebugParameter(
"motor_rod_joint_far_left", -0.1, 0.1, 0)
linear_motor_mid_left_ctrl = p.addUserDebugParameter(
"motor_rod_joint_mid_left", -0.1, 0.1, 0)
linear_motor_far_right_ctrl = p.addUserDebugParameter(
"motor_rod_joint_far_right", -0.1, 0.1, 0)
linear_motor_mid_right_ctrl = p.addUserDebugParameter(
"motor_rod_joint_mid_right", -0.1, 0.1, 0)
# Print contact
# Manual Control
p.setGravity(0, 0, -9.8)
useRealTime = 0
p.setRealTimeSimulation(useRealTime)
p.setGravity(0, 0, -10)
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,
dataDir = '/home/biomed/Art/bullet3/data'
conid = p.connect(p.SHARED_MEMORY)
if (conid<0):
p.connect(p.GUI)
p.setInternalSimFlags(0)
p.resetSimulation()
plane = p.loadURDF(os.path.join(dataDir, "plane.urdf"),useMaximalCoordinates=useMaximalCoordinates)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
gravXid = p.addUserDebugParameter("gravityX",-10,10,0)
gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
gravZid = p.addUserDebugParameter("gravityZ",-10,10,-5)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
centers = 0
mass = 1
visualShapeId = -1
n_links = 5
link_Masses=[1] * n_links
linkCollisionShapeIndices=[colSphereId] * n_links
enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5,
enableCollision)
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 = []
import pybullet as p
import time
useMaximalCoordinates = False
p.connect(p.GUI)
pole = p.loadURDF("cartpole.urdf", useMaximalCoordinates=useMaximalCoordinates)
for i in range(p.getNumJoints(pole)):
#disable default constraint-based motors
p.setJointMotorControl2(pole,
i,
p.POSITION_CONTROL,
targetPosition=0,
force=0)
print("joint", i, "=", p.getJointInfo(pole, i))
desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0)
kpCartId = p.addUserDebugParameter("kpCart", 0, 500, 300)
kdCartId = p.addUserDebugParameter("kdCart", 0, 300, 150)
maxForceCartId = p.addUserDebugParameter("maxForceCart", 0, 5000, 1000)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole", -10, 10, 0)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole", -10, 10, 0)
kpPoleId = p.addUserDebugParameter("kpPole", 0, 500, 200)
kdPoleId = p.addUserDebugParameter("kdPole", 0, 300, 100)
maxForcePoleId = p.addUserDebugParameter("maxForcePole", 0, 5000, 1000)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0, 0, -10)
import pybullet as p
import time
import pybullet_data
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
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)
cubeStartPos = [0, 0, 0.5] # RGB = xyz
cubeStartOrientation = p.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
# rotating a standing cylinder around the y axis, puts it flat onto the x axis
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)
# pybullet 내장 model 들을 로드
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setTimeStep(1 / 200)
cubeStartPos = [0, 0, 1]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
wheel_indices = [0, 1, 2, 3]
hinge_indicies = [0, 2]
planeID = p.loadURDF("plane.urdf")
carID = p.loadURDF("./simplecar.urdf", cubeStartPos, cubeStartOrientation)
number_of_joints = p.getNumJoints(carID)
angle = p.addUserDebugParameter("Steering", -0.5, 0.5, 0) # 디버그 창의 좌표
throttle = p.addUserDebugParameter("Throttle", 0, 20, 0)
for i in range(10000):
# ----- code here
user_angle = p.readUserDebugParameter(angle)
user_throttle = p.readUserDebugParameter(throttle)
for joint_index in wheel_indices:
p.setJointMotorControl2(carID,
joint_index,
p.VELOCITY_CONTROL,
targetVelocity=user_throttle)
# ----- code end
p.stepSimulation() # 명령 적용 후 rendering 1번.
time.sleep(1 / 200)
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
import time
import resources
from classifier import Classifier
from engine import Engine, Cube, Colors
import pybullet as p
import numpy as np
import random
if __name__ == "__main__":
engine = Engine(gui=True)
classifier = Classifier()
classifier.load(resources.CNNS)
rgb_ids = (p.addUserDebugParameter("red", 0, 1, 1),
p.addUserDebugParameter("green", 0, 1, 0),
p.addUserDebugParameter("blue", 0, 1, 0))
theta, rho, angle = (p.addUserDebugParameter("theta", 0, 360, 0),
p.addUserDebugParameter("rho", 0.5, 1.5, 1),
p.addUserDebugParameter("angle", 0, 360, 0))
cube_size = p.addUserDebugParameter("size", 0.5, 2.0, 1.0)
last_cube_update = time.time()
def update_debug_parameters():
global last_cube_update
cube = None
last_pos, last_angle, last_size = (None, None, None)
while p.isConnected():
rgb = [p.readUserDebugParameter(id) for id in rgb_ids] + [1]
_theta, _rho, _angle = (p.readUserDebugParameter(theta) / 180.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)
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]):
x in field for x in ['rear', 'wheel']):
power_wheels.append(i)
break
print 'power_wheels', power_wheels
print 'steer_wheels', steer_wheels
for wheel in inactive_wheels:
p.setJointMotorControl2(car,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
steering = [4, 6]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -10, 10, 0)
maxForceSlider = p.addUserDebugParameter("maxForce", 0, 10, 10)
steeringSlider = p.addUserDebugParameter("steering", -0.5, 0.5, 0)
while (True):
maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = p.readUserDebugParameter(steeringSlider)
#print(targetVelocity)
for wheel in power_wheels:
p.setJointMotorControl2(car,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=maxForce)
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("plane.urdf")
humanoid = p.loadURDF("cassie/urdf/cassie_collide.urdf", [0, 0, 0.8],
useFixedBase=False)
gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
jointIds = []
paramIds = []
p.setPhysicsEngineParameter(numSolverIterations=100)
p.changeDynamics(humanoid, -1, linearDamping=0, angularDamping=0)
jointAngles = [
0, 0, 1.0204, -1.97, -0.084, 2.06, -1.9, 0, 0, 1.0204, -1.97, -0.084, 2.06,
-1.9, 0
]
activeJoint = 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,
jointAngles[activeJoint]))
p.resetJointState(humanoid, j, jointAngles[activeJoint])
print("joint[", joint, "]=", pybullet.getJointInfo(obj, joint))
pybullet.setJointMotorControl2(obj,
joint,
pybullet.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
pybullet.getJointInfo(obj, joint)
#>>>>>>>>>>>>>>>>>>>>>>>>-------set constraints--------<<<<<<<<<<<<<<<<<<<<<<#
# <constraint between different objects and child links>
# c = p.createConstraint(obj, 9, obj,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
# p.changeConstraint(c, gearRatio=1, maxForce=10000)
#>>>>>>>>>>>>>>>>>>>>>>>>-------Interactions for Vis--------<<<<<<<<<<<<<<<<<<<<<<#
# targetVelocitySlider = pybullet.addUserDebugParameter("wheelVelocity",-50,50,0)
# maxForceSlider = pybullet.addUserDebugParameter("maxForce",0,50,20)
steeringSlider = pybullet.addUserDebugParameter("steering", -1, 1,
0) # start value with 0
camPosXSlider = pybullet.addUserDebugParameter("X", -1, 1, 0)
camPosYSlider = pybullet.addUserDebugParameter("Y", -1, 1, 0)
camPosZSlider = pybullet.addUserDebugParameter("Z", -1, 1, 0)
if platform.uname()[0] == 'Darwin':
print("Now it knows it's in my local Mac")
base_path = '/Users/DragonX/Downloads/NR_WORK/6DPOSE/partnet/pose_articulated'
elif platform.uname()[1] == 'viz1':
base_path = '/home/xiaolong/Downloads/6DPOSE/partnet/pose_articulated'
elif platform.uname()[1] == 'vllab3':
base_path = '/mnt/data/lxiaol9/rbo'
else:
base_path = '/work/cascades/lxiaol9/6DPOSE/partnet/pose_articulated'
simu_cnt = 0
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()
def __init__(self):
self.maxSl = 2
self.bodyPos = (0, 100, 0)
self.bodyRot = (0, 0, 0)
self.t0 = 0 # senseless i guess
self.t1 = 510
self.t2 = 00
self.t3 = 185
self.Sl = 0.0
self.Sw = 0
self.Sh = 60
self.Sa = 0
self.Spf = 87
self.Spr = 77
self.IDspurFront = p.addUserDebugParameter("spur front", 20, 150,
self.Spf)
self.IDspurRear = p.addUserDebugParameter("spur rear", 20, 150,
self.Spr)
self.IDstepLength = p.addUserDebugParameter("step length", -150, 150,
self.Sl)
self.IDstepWidth = p.addUserDebugParameter("step width", -150, 150,
self.Sw)
self.IDstepHeight = p.addUserDebugParameter("step height", 0, 150,
self.Sh)
self.IDstepAlpha = p.addUserDebugParameter("step alpha", -90, 90,
self.Sa)
self.IDt0 = p.addUserDebugParameter("t0", 0, 1000, self.t0)
self.IDt1 = p.addUserDebugParameter("t1", 0, 1000, self.t1)
self.IDt2 = p.addUserDebugParameter("t2", 0, 1000, self.t2)
self.IDt3 = p.addUserDebugParameter("t3", 0, 1000, self.t3)
self.IDfrontOffset = p.addUserDebugParameter("front Offset", 0, 200,
120)
self.IDrearOffset = p.addUserDebugParameter("rear Offset", 0, 200, 50)
self.Rc = [-50, 0, 0, 1] # rotation center
# base (link) -> joint -> chassis (link)
# -> joint_wheel_left -> wheel_left (link)
# -> joint_wheel_right -> wheel_right (link)
# (i.e. it's an acyclical graph; a link can have many joints attached)
for i in range(p.getNumJoints(robot)):
print(p.getJointInfo(robot, i))
# Indices of all the motors we can actually move. This information comes from the print statemtnt above.
motors = [3, 4, 6, 8, 10, 12]
# Container for debug inputs
debugParams = []
# In the user interface, create a slider for each motor to control them separately.
for i in range(len(motors)):
motor = p.addUserDebugParameter("motor{}".format(i + 1), -1, 1, 0)
debugParams.append(motor)
start = time.time()
# Stepping frequency * 30 = run the simulation for 30 seconds
for i in range(frequency * 30):
motorPos = []
# At each timestep, read the values from the motor sliders and set the motors to that position
# via position control. The motors have an internal PID controller, that will take over and calculate
# the force and direction that is necessary to reach the new target position from the current position it's in.
# In practice, this doesn't need to be called so often. As soon as you send a new motor position command to the
# motor via `setJointMotorControl2`, it will go there over time. No need to send the same command twice.
# The only reason it's here in the loop, is so that user can slide the motor values around and the robot moves in
# realtime.
def __init__(self,
urdf_root=pybullet_data.getDataPath(),
renders=False,
is_discrete=True,
name="mobile_robot",
max_distance=1.6,
shape_reward=False,
record_data=False,
srl_model="raw_pixels",
random_target=False,
force_down=True,
state_dim=-1,
learn_states=False,
verbose=False,
save_path='srl_zoo/data/',
env_rank=0,
srl_pipe=None,
fpv=False,
img_shape=None,
**_):
super(MobileRobotGymEnv, self).__init__(srl_model=srl_model,
relative_pos=RELATIVE_POS,
env_rank=env_rank,
srl_pipe=srl_pipe)
self._timestep = 1. / 240.
self._urdf_root = urdf_root
self._observation = []
self._env_step_counter = 0
self._renders = renders
self.img_shape = img_shape # channel first !!
if self.img_shape is None:
self._width = RENDER_WIDTH
self._height = RENDER_HEIGHT
else:
self._height, self._width = self.img_shape[1:]
self._cam_dist = 4.4
self._cam_yaw = 90
self._cam_pitch = -90
self._cam_roll = 0
self._max_distance = max_distance
self._shape_reward = shape_reward
self._random_target = random_target
self._force_down = force_down
self.camera_target_pos = (2, 2, 0)
self._is_discrete = is_discrete
self.terminated = False
self.renderer = p.ER_TINY_RENDERER
self.debug = False
self.n_contacts = 0
self.state_dim = state_dim
self.relative_pos = RELATIVE_POS
self.cuda = th.cuda.is_available()
self.saver = None
self.verbose = verbose
self.max_steps = MAX_STEPS
self.robot_pos = np.zeros(3)
self.robot_uid = None
self.target_pos = np.zeros(3)
self.target_uid = None
# Boundaries of the square env
self._min_x, self._max_x = 0, 4
self._min_y, self._max_y = 0, 4
self.has_bumped = False # Used for handling collisions
self.collision_margin = 0.1
self.walls = None
self.fpv = fpv
self.srl_model = srl_model
if record_data:
self.saver = EpisodeSaver(name,
max_distance,
state_dim,
globals_=getGlobals(),
relative_pos=RELATIVE_POS,
learn_states=learn_states,
path=save_path)
if self._renders:
client_id = p.connect(p.SHARED_MEMORY)
if client_id < 0:
p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
self.debug = True
# Debug sliders for moving the camera
self.x_slider = p.addUserDebugParameter("x_slider", -10, 10,
self.camera_target_pos[0])
self.y_slider = p.addUserDebugParameter("y_slider", -10, 10,
self.camera_target_pos[1])
self.z_slider = p.addUserDebugParameter("z_slider", -10, 10,
self.camera_target_pos[2])
self.dist_slider = p.addUserDebugParameter("cam_dist", 0, 10,
self._cam_dist)
self.yaw_slider = p.addUserDebugParameter("cam_yaw", -180, 180,
self._cam_yaw)
self.pitch_slider = p.addUserDebugParameter(
"cam_pitch", -180, 180, self._cam_pitch)
else:
p.connect(p.DIRECT)
global CONNECTED_TO_SIMULATOR
CONNECTED_TO_SIMULATOR = True
if self._is_discrete:
self.action_space = spaces.Discrete(N_DISCRETE_ACTIONS)
else:
self.action_space = spaces.Box(low=-1,
high=1,
shape=(2, ),
dtype=np.float32)
if self.srl_model == "ground_truth":
self.state_dim = self.getGroundTruthDim()
if self.srl_model == "raw_pixels":
self.observation_space = spaces.Box(low=0,
high=255,
shape=(self._height,
self._width, 3),
dtype=np.uint8)
else:
self.observation_space = spaces.Box(low=-np.inf,
high=np.inf,
shape=(self.state_dim, ),
dtype=np.float32)
limitVal = 2*pi
legpos = 3./4.*pi
legposS = 0
legposSright = 0#-0.3
legposSleft = 0#0.3
defaultERP=0.4
maxMotorForce = 5000
maxGearForce = 10000
jointFriction = 0.1
p.connect(p.GUI)
amplitudeId= p.addUserDebugParameter("amplitude",0,3.14,0.5)
timeScaleId = p.addUserDebugParameter("timeScale",0,10,1)
jointTypeNames={}
jointTypeNames[p.JOINT_REVOLUTE]="JOINT_REVOLUTE"
jointTypeNames[p.JOINT_FIXED]="JOINT_FIXED"
p.setPhysicsEngineParameter(numSolverIterations=100)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
jointNamesToIndex={}
def __init__(self,
gym_config,
robot_class=None,
env_sensors=None,
robot_sensors=None,
task=None,
env_randomizers=None):
"""Initializes the locomotion gym environment.
Args:
gym_config: An instance of LocomotionGymConfig.
robot_class: A class of a robot. We provide a class rather than an
instance due to hard_reset functionality. Parameters are expected to be
configured with gin.
sensors: A list of environmental sensors for observation.
task: A callable function/class to calculate the reward and termination
condition. Takes the gym env as the argument when calling.
env_randomizers: A list of EnvRandomizer(s). An EnvRandomizer may
randomize the physical property of minitaur, change the terrrain during
reset(), or add perturbation forces during step().
Raises:
ValueError: If the num_action_repeat is less than 1.
"""
self.seed()
self._gym_config = gym_config
self._robot_class = robot_class
self._robot_sensors = robot_sensors
self._sensors = env_sensors if env_sensors is not None else list()
if self._robot_class is None:
raise ValueError('robot_class cannot be None.')
# A dictionary containing the objects in the world other than the robot.
self._world_dict = {}
self._task = task
self._env_randomizers = env_randomizers if env_randomizers else []
# This is a workaround due to the issue in b/130128505#comment5
if isinstance(self._task, sensor.Sensor):
self._sensors.append(self._task)
# Simulation related parameters.
self._num_action_repeat = gym_config.simulation_parameters.num_action_repeat
self._on_rack = gym_config.simulation_parameters.robot_on_rack
if self._num_action_repeat < 1:
raise ValueError('number of action repeats should be at least 1.')
self._sim_time_step = gym_config.simulation_parameters.sim_time_step_s
self._env_time_step = self._num_action_repeat * self._sim_time_step
self._env_step_counter = 0
self._num_bullet_solver_iterations = int(
_NUM_SIMULATION_ITERATION_STEPS / self._num_action_repeat)
self._is_render = gym_config.simulation_parameters.enable_rendering
# The wall-clock time at which the last frame is rendered.
self._last_frame_time = 0.0
self._show_reference_id = -1
if self._is_render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_GUI,
gym_config.simulation_parameters.enable_rendering_gui)
self._show_reference_id = pybullet.addUserDebugParameter(
"show reference", 0, 1, self._task._draw_ref_model_alpha)
self._delay_id = pybullet.addUserDebugParameter("delay", 0, 0.3, 0)
else:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.DIRECT)
self._pybullet_client.setAdditionalSearchPath(pd.getDataPath())
if gym_config.simulation_parameters.egl_rendering:
self._pybullet_client.loadPlugin('eglRendererPlugin')
# The action list contains the name of all actions.
self._action_list = []
action_upper_bound = []
action_lower_bound = []
action_config = robot_class.ACTION_CONFIG
for action in action_config:
self._action_list.append(action.name)
action_upper_bound.append(action.upper_bound)
action_lower_bound.append(action.lower_bound)
self.action_space = spaces.Box(np.array(action_lower_bound),
np.array(action_upper_bound),
dtype=np.float32)
# Set the default render options.
self._camera_dist = gym_config.simulation_parameters.camera_distance
self._camera_yaw = gym_config.simulation_parameters.camera_yaw
self._camera_pitch = gym_config.simulation_parameters.camera_pitch
self._render_width = gym_config.simulation_parameters.render_width
self._render_height = gym_config.simulation_parameters.render_height
self._hard_reset = True
self.reset()
self._hard_reset = gym_config.simulation_parameters.enable_hard_reset
# Construct the observation space from the list of sensors. Note that we
# will reconstruct the observation_space after the robot is created.
self.observation_space = (
space_utils.convert_sensors_to_gym_space_dictionary(
self.all_sensors()))
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)
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.)
