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