def motor_set_speed(self, node, speed): targetVel = speed if node == 3: p.setJointMotorControl(self.neck, 2, p.POSITION_CONTROL, targetVel, self.maxForce) if node == 5: p.setJointMotorControl(self.neck, 1, p.POSITION_CONTROL, targetVel, self.maxForce)
import pybullet_data p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf") p.setGravity(0, 0, -10) huskypos = [0, 0, 0.1] husky = p.loadURDF("husky/husky.urdf", huskypos[0], huskypos[1], huskypos[2]) numJoints = p.getNumJoints(husky) for joint in range(numJoints): print(p.getJointInfo(husky, joint)) targetVel = 10 #rad/s maxForce = 100 #Newton for joint in range(2, 6): p.setJointMotorControl(husky, joint, p.VELOCITY_CONTROL, targetVel, maxForce) for step in range(300): p.stepSimulation() targetVel = -10 for joint in range(2, 6): p.setJointMotorControl(husky, joint, p.VELOCITY_CONTROL, targetVel, maxForce) for step in range(400): p.stepSimulation() p.getContactPoints(husky) p.disconnect()
#!/usr/bin/env python3 import pybullet as p from time import sleep TIME_STEP = 0.001 physicsClient = p.connect(p.GUI) p.setGravity(0, 0, -9.8) p.setTimeStep(TIME_STEP) planeId = p.loadURDF("URDF/plane.urdf", [0, 0, 0]) RobotId = p.loadURDF("URDF/simple_humanoid.urdf", [0, 0, 0]) for joint in range(p.getNumJoints(RobotId)): p.setJointMotorControl(RobotId, joint, p.POSITION_CONTROL, 0) index = { p.getBodyInfo(RobotId)[0].decode('UTF-8'): -1, } for id in range(p.getNumJoints(RobotId)): index[p.getJointInfo(RobotId, id)[12].decode('UTF-8')] = id print(index) angle = 0 angle2 = 0.19 velocity = 1.5 velocity2 = 1.5 while p.isConnected(): angle += velocity * TIME_STEP
import time import math p.connect(p.SHARED_MEMORY) p.loadURDF("plane.urdf") quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3) #p.getNumJoints(1) #right front leg p.resetJointState(quadruped,0,1.57) p.resetJointState(quadruped,2,-2.2) p.resetJointState(quadruped,3,-1.57) p.resetJointState(quadruped,5,2.2) p.createConstraint(quadruped,2,quadruped,5,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,1.57,1) p.setJointMotorControl(quadruped,1,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,2,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1) p.setJointMotorControl(quadruped,4,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,5,p.VELOCITY_CONTROL,0,0) #left front leg p.resetJointState(quadruped,6,1.57) p.resetJointState(quadruped,8,-2.2) p.resetJointState(quadruped,9,-1.57) p.resetJointState(quadruped,11,2.2) p.createConstraint(quadruped,8,quadruped,11,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2]) p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,1.57,1) p.setJointMotorControl(quadruped,7,p.VELOCITY_CONTROL,0,0)
#!/usr/bin/env python3 import pybullet as p from time import sleep TIME_STEP = 0.001 physicsClient = p.connect(p.GUI) p.setGravity(0, 0, -9.8) p.setTimeStep(TIME_STEP) planeId = p.loadURDF("URDF/plane.urdf", [0, 0, 0]) RobotId = p.loadURDF("URDF/gankenkun.urdf", [0, 0, 0]) for joint in range(p.getNumJoints(RobotId)): p.setJointMotorControl(RobotId, joint, p.POSITION_CONTROL, 0) index = { p.getBodyInfo(RobotId)[0].decode('UTF-8'): -1, } for id in range(p.getNumJoints(RobotId)): index[p.getJointInfo(RobotId, id)[12].decode('UTF-8')] = id p.setJointMotorControl(RobotId, index['left_lower_arm_link'], p.POSITION_CONTROL, -0.5) p.setJointMotorControl(RobotId, index['right_lower_arm_link'], p.POSITION_CONTROL, -0.5) p.setJointMotorControl(RobotId, index['left_upper_arm_link'], p.POSITION_CONTROL, -0.2) p.setJointMotorControl(RobotId, index['right_upper_arm_link'], p.POSITION_CONTROL, -0.2)
# Number of joints num_joints = p.getNumJoints(husky) # For each joint for joint in range(num_joints): print(p.getJointInfo(husky, joint)) # end for # Target velocity target_velocity = 10 max_force = 100 # for join 2 to 5 for joint in range(2, 6): p.setJointMotorControl(husky, joint, p.VELOCITY_CONTROL, target_velocity, max_force) # end for # Simulate for 300 steps for step in range(300): p.stepSimulation() # end for # Target velocity target_velocity = -10 # For join 2 to 5 for step in range(2, 6): p.setJointMotorControl(husky, joint, p.VELOCITY_CONTROL, target_velocity, max_force) # end for
nozzleYTgt = rollPID.control(rocketYPR[0], rocketVel[1][0], -rollTgt) #print(pitchTgt) #Debug """ if i < 1200: #Call Engine Model, apply thrust thrust,mdot = R.setThrottle(R,throttle) else: thrust,mdot = R.setThrottle(R,p.readUserDebugParameter(throttleInput)) p.setJointMotorControl(rocket,1,p.POSITION_CONTROL,p.readUserDebugParameter(nozzleInput)) """ thrust, mdot = R.setThrottle(R, throttle) p.setJointMotorControl(rocket, 1, p.POSITION_CONTROL, (-nozzleYTgt) * 0.2) p.setJointMotorControl(rocket, 2, p.POSITION_CONTROL, (-nozzleXTgt) * 0.2) p.applyExternalForce(rocket, 2, thrust, thrustAtNozzle, p.LINK_FRAME) #print(rocketVel[1]) #print(rollTgt) #RCS if RCSEnable: RCSThrustX, RCSmdotX = RCS.setThrottle(nozzleXTgt) RCSThrustY, RCSmdotY = RCS.setThrottle(nozzleYTgt) mdot = mdot + RCSmdotX + RCSmdotY if nozzleXTgt < 0: p.applyExternalForce(rocket, rcs_front, RCSThrustX, [0, 0, 0], p.LINK_FRAME)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally p.setGravity(0, 0, -9.81) planeId = p.loadURDF("plane.urdf") cubeStartPos = [0, 0, 2] cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) boxId = p.loadURDF("rocket.urdf", cubeStartPos, cubeStartOrientation) for i in range(10000): loc = [0, 0, -0.15] force = [0, 0, 250] torque = [0, i / 240, 0] #print(force) p.applyExternalForce(boxId, 1, force, loc, p.LINK_FRAME) #p.applyExternalTorque(boxId,-1,torque,p.LINK_FRAME) p.setJointMotorControl(boxId, 4, p.POSITION_CONTROL, 0.5) if i > 240 * 1: p.setJointMotorControl(boxId, 4, p.POSITION_CONTROL, -0.5) cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) """if cubePos[2] > 10: p.changeDynamics(boxId, -1, mass=50) p.changeDynamics(boxId, 0, )""" #p.resetDebugVisualizerCamera(5,30,-30,cubePos) p.stepSimulation() time.sleep(1. / 240.) cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) print(cubePos, cubeOrn) p.disconnect()
#Controller throttle = heightPID.control(rocketPos[2], rocketVel[0][2], 1.5) pitchTgt = XPID.control(rocketPos[0], rocketVel[0][0], 0) nozzleTgt = pitchPID.control(rocketYPR[1], rocketVel[1][1], pitchTgt) #print(nozzleTgt) #Debug if i < 1200: #Call Engine Model, apply thrust thrust, mdot = R.setThrottle(R, throttle) else: thrust, mdot = R.setThrottle(R, p.readUserDebugParameter(throttleInput)) p.setJointMotorControl(rocket, 1, p.POSITION_CONTROL, p.readUserDebugParameter(nozzleInput)) thrust, mdot = R.setThrottle(R, throttle) p.setJointMotorControl(rocket, 1, p.POSITION_CONTROL, (-nozzleTgt) * 0.2) p.applyExternalForce(rocket, 1, thrust, thrustAtNozzle, p.LINK_FRAME) #RCS if RCSEnable: RCSThrust, RCSmdot = RCS.setThrottle(nozzleTgt) mdot = mdot + RCSmdot if nozzleTgt < 0: p.applyExternalForce(rocket, rcs_front, RCSThrust, [0, 0, 0], p.LINK_FRAME) lineStuff = p.getLinkState(rocket, rcs_front)
import pybullet as p p.connect(p.GUI) #or p.SHARED_MEMORY or p.DIRECT p.loadURDF("plane.urdf") p.setGravity(0,0,-10) huskypos = [0,0,0.1] husky = p.loadURDF("husky/husky.urdf",huskypos[0],huskypos[1],huskypos[2]) numJoints = p.getNumJoints(husky) for joint in range (numJoints) : print (p.getJointInfo(husky,joint)) targetVel = 10 #rad/s maxForce = 100 #Newton for joint in range (2,6) : p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce) for step in range (300): p.stepSimulation() targetVel=-10 for joint in range (2,6) : p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce) for step in range (400): p.stepSimulation() p.getContactPointData(husky) p.disconnect()
plane = p.loadURDF("plane.urdf", [0, 0, -0.001]) robot = p.loadURDF(urdf_path, [0, 0, 0.001], useFixedBase=True) dof = p.getNumJoints(robot) joint_positions = [0, 0, 0] * dof sliders = [] for j in range(dof): joint_info = p.getJointInfo(robot, j) print(joint_info) sliders.append( p.addUserDebugParameter(f"Joint {j}", -np.pi / 2, np.pi / 2, 0)) p.setJointMotorControl2(robot, j, p.VELOCITY_CONTROL, force=0) p.changeDynamics(robot, j, jointLowerLimit=-np.pi / 2, jointUpperLimit=np.pi / 2) while True: for j in range(dof): joint_positions[j] = p.readUserDebugParameter(sliders[j]) p.setJointMotorControl(robot, j, p.POSITION_CONTROL, joint_positions[j]) p.stepSimulation() time.sleep(dt) p.disconnect() sys.exit(0)
if (toeConstraint): cid = p.createConstraint(quadruped, knee_front_leftR_link, quadruped, knee_front_leftL_link, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1]) p.changeConstraint(cid, maxForce=maxKneeForce) cid = p.createConstraint(quadruped, knee_front_rightR_link, quadruped, knee_front_rightL_link, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1]) p.changeConstraint(cid, maxForce=maxKneeForce) cid = p.createConstraint(quadruped, knee_back_leftR_link, quadruped, knee_back_leftL_link, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1]) p.changeConstraint(cid, maxForce=maxKneeForce) cid = p.createConstraint(quadruped, knee_back_rightR_link, quadruped, knee_back_rightL_link, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1]) p.changeConstraint(cid, maxForce=maxKneeForce) if (1): p.setJointMotorControl(quadruped, knee_front_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_rightL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_rightR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_rightL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_rightR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
def enable_joint(self): for joint in range(self.numJoints): print(p.getJointInfo(self.robot, joint)) p.setJointMotorControl(self.robot, joint, p.POSITION_CONTROL, self.targetVel, self.maxForce)
def set_angles(self, angles_info): for j, v in angles_info.items(): if j not in self.joints: AssertionError("Invalid joint name " + j) continue p.setJointMotorControl(self.robot, self.joints.index(j), p.POSITION_CONTROL, v, self.maxForce)
p.changeConstraint(cid, maxForce=maxKneeForce) cid = p.createConstraint(quadruped, knee_front_rightR_link, quadruped, knee_front_rightL_link, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1]) p.changeConstraint(cid, maxForce=maxKneeForce) cid = p.createConstraint(quadruped, knee_back_leftR_link, quadruped, knee_back_leftL_link, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1]) p.changeConstraint(cid, maxForce=maxKneeForce) cid = p.createConstraint(quadruped, knee_back_rightR_link, quadruped, knee_back_rightL_link, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1]) p.changeConstraint(cid, maxForce=maxKneeForce) if (1): p.setJointMotorControl(quadruped, knee_front_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_rightL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_rightR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_rightL_link,
#!/usr/bin/env python3 import pybullet as p from time import sleep TIME_STEP = 0.001 physicsClient = p.connect(p.GUI) p.setGravity(0, 0, -9.8) p.setTimeStep(TIME_STEP) planeId = p.loadURDF("URDF/plane.urdf", [0, 0, 0]) RobotId = p.loadURDF("URDF/gankenkun.urdf", [0, 0, 0]) for joint in range(p.getNumJoints(RobotId)): p.setJointMotorControl(RobotId, joint, p.POSITION_CONTROL, 0) index = { p.getBodyInfo(RobotId)[0].decode('UTF-8'): -1, } for id in range(p.getNumJoints(RobotId)): index[p.getJointInfo(RobotId, id)[12].decode('UTF-8')] = id print(index) p.setJointMotorControl(RobotId, index['left_lower_arm_link'], p.POSITION_CONTROL, -0.5) p.setJointMotorControl(RobotId, index['right_lower_arm_link'], p.POSITION_CONTROL, -0.5) p.setJointMotorControl(RobotId, index['left_upper_arm_link'], p.POSITION_CONTROL, -0.2)
import math p.connect(p.SHARED_MEMORY) p.loadURDF("plane.urdf") quadruped = p.loadURDF("quadruped/quadruped.urdf", 0, 0, .3) #p.getNumJoints(1) #right front leg p.resetJointState(quadruped, 0, 1.57) p.resetJointState(quadruped, 2, -2.2) p.resetJointState(quadruped, 3, -1.57) p.resetJointState(quadruped, 5, 2.2) p.createConstraint(quadruped, 2, quadruped, 5, 3, [0, 0, 0], [0, 0.01, 0.2], [0, -0.015, 0.2]) p.setJointMotorControl(quadruped, 0, p.POSITION_CONTROL, 1.57, 1) p.setJointMotorControl(quadruped, 1, p.VELOCITY_CONTROL, 0, 0) p.setJointMotorControl(quadruped, 2, p.VELOCITY_CONTROL, 0, 0) p.setJointMotorControl(quadruped, 3, p.POSITION_CONTROL, -1.57, 1) p.setJointMotorControl(quadruped, 4, p.VELOCITY_CONTROL, 0, 0) p.setJointMotorControl(quadruped, 5, p.VELOCITY_CONTROL, 0, 0) #left front leg p.resetJointState(quadruped, 6, 1.57) p.resetJointState(quadruped, 8, -2.2) p.resetJointState(quadruped, 9, -1.57) p.resetJointState(quadruped, 11, 2.2) p.createConstraint(quadruped, 8, quadruped, 11, 3, [0, 0, 0], [0, -0.01, 0.2], [0, 0.015, 0.2]) p.setJointMotorControl(quadruped, 6, p.POSITION_CONTROL, 1.57, 1)
motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint'] hip_leftR_link = jointNameToId['hip_leftR_link'] knee_back_leftR_link = jointNameToId['knee_back_leftR_link'] motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint'] motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] #p.getNumJoints(1) p.resetJointState(quadruped, motor_front_rightR_joint, 1.57) p.resetJointState(quadruped, knee_front_rightR_link, -2.2) p.resetJointState(quadruped, motor_front_rightL_joint, 1.57) p.resetJointState(quadruped, knee_front_rightL_link, -2.2) p.createConstraint(quadruped, knee_front_rightR_link, quadruped, knee_front_rightL_link, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.2], [0, 0.01, 0.2]) p.setJointMotorControl(quadruped, knee_front_rightR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_rightL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.resetJointState(quadruped, motor_front_leftR_joint, -1.57) p.resetJointState(quadruped, knee_front_leftR_link, 2.2) p.resetJointState(quadruped, motor_front_leftL_joint, -1.57) p.resetJointState(quadruped, knee_front_leftL_link, 2.2) p.createConstraint(quadruped, knee_front_leftR_link, quadruped, knee_front_leftL_link, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.2], [0, 0.01, 0.2]) p.setJointMotorControl(quadruped, knee_front_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)