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.)