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)
