def __init__(self, pelvic_interval, legUp_length, legDown_length,
footJoint_to_bottom, hip_shift, sittinghip_shift, swayLength,
stepTime, robot, physicsClient, motor_force):
self._pelvic_interval = pelvic_interval # distance between the two hip joints
self._legUp_length = legUp_length # hip joint to knee joint
self._legDown_length = legDown_length # knee joint to foot joint
self._footJoint_to_bottom = footJoint_to_bottom # foot joint to bottom
self._hip_shift = hip_shift # hip_shift of lift of a foot #temp in original (temp)
self._sittinghip_shift = sittinghip_shift # sit hip_shift. increase this will make leg more fold. too high or too low makes an error (z0)
self._swayLength = swayLength # foot sway length, length by which feet move in one step (x0)
self._stepTime = stepTime # time taken for one step (t0)
self.trajectory = trajectoryGenerator.TrajectoryGenerator(
pelvic_interval, legUp_length, legDown_length,
footJoint_to_bottom) # initializing trajectory generator
self.trajectory.setTrajectoryParameters(
hip_shift, sittinghip_shift, swayLength,
stepTime) # setting up walk/trajectory parameters
self.controller = motorController.MotorController(
robot, physicsClient, motor_force
) # setting up motor controller to control motor positions
p.setGravity(0, 0, 0)
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=10,
cameraPitch=-0,
cameraTargetPosition=[0.4, 0, 0.1])
planeID = p.loadURDF("plane.urdf")
robotID = p.loadURDF(os.path.abspath(os.path.dirname(__file__)) +
'/humanoid_leg_12dof.8.urdf', [0, 0, 0.31],
p.getQuaternionFromEuler([0, 0, 0]),
useFixedBase=False)
motorsController = motorController.MotorController(robotID, physicsClient,
fixedTimeStep, motor_kp,
motor_kd, motor_torque,
motor_max_velocity)
print(motorsController.getRevoluteJoint_nameToId())
# %%
motorController.MotorController()
walk = walkGenerator.WalkGenerator()
walk.setWalkParameter(bodyMovePoint=8,
legMovePoint=8,
height=50,
stride=90,
sit=50,
swayBody=30,
swayFoot=0,
bodyPositionForwardPlus=5,
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=10,
cameraPitch=-5,
cameraTargetPosition=[0.3, 0.5, 0.1],
physicsClientId=physicsClient)
planeId = p.loadSDF(
'stadium.sdf') # or p.loadURDF('samurai.urdf') # p.loadURDF('plane.urdf')
robot = p.loadURDF(os.path.abspath(os.path.dirname(__file__)) +
'/humanoid_leg_12dof.8.urdf', [0, 0, 0.31],
p.getQuaternionFromEuler([0, 0, 0]),
useFixedBase=False)
controller = motorController.MotorController(robot, physicsClient,
fixedTimeStep, motor_kp, motor_kd,
motor_torque, motor_max_velocity)
walk = walkGenerator.WalkGenerator()
walk.setWalkParameter(bodyMovePoint=8,
legMovePoint=8,
height=50,
stride=90,
sit=40,
swayBody=30,
swayFoot=0,
bodyPositionForwardPlus=5,
swayShift=3,
liftPush=0.4,
landPull=0.6,
timeStep=0.06,
p.setGravity(0, 0, 0)
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=10,
cameraPitch=-0,
cameraTargetPosition=[0.4, 0, 0.1])
planeId = p.loadURDF("plane.urdf")
humanoid = p.loadURDF(
r"C:\Users\Scientist\ml_robotics\bipedal_robot_simulation\humanoid_leg_12dof.7.urdf",
[0, 0, 0.1],
p.getQuaternionFromEuler([0, 0, 0]),
useFixedBase=False)
motorsController = motorController.MotorController(humanoid, physicsClient,
fixedTimeStep, motor_kp,
motor_kd, motor_torque,
motor_max_velocity)
print(motorsController.getRevoluteJoint_nameToId())
walk = walkGenerator.WalkGenerator()
walk.setWalkParameter(bodyMovePoint=8,
legMovePoint=8,
h=50,
l=90,
sit=50,
swayBody=45,
swayFoot=0,
bodyPositionXPlus=5,
swayShift=3,
weightStart=0.5,
weightEnd=0.7,
configFile = "config.ini"
pi = pigpio.pi()
retryCount = 0
while not pi.connected and retryCount < 5000:
sleep(0.001)
pi = pigpio.pi()
retryCount += 1
if retryCount >= 5000:
print("Could not connect to pigpiod. Exiting")
cleanup()
JS = joyStick.JoyStick()
motorController = motorController.MotorController(pi, configFile)
servoController = servoController.ServoController(pi, configFile)
mode = "SlideLimitSetup"
while True:
JS.update()
if JS.startBtn and JS.backBtn: # This is the signal to quit
cleanup()
if mode == "SlideLimitSetup":
motorController.setSpeed(JS.slideAxis)
if JS.yBtn:
motorController.setLimit()
robotStartOrn = p.getQuaternionFromEuler([1.57, 0, 0])
# load objects - plane and robot
plane = p.loadURDF("plane.urdf")
robotID = p.loadURDF(
"urdf/moveTesting.urdf", robotStartPosition, robotStartOrn, useFixedBase=False)
motor_kd = 0.5
motor_kp = 0.5
motor_torque = 1.5
motor_max_velocity = 5.0
timeStep = 1./240.
jd = [0.1]
motor_c = m.MotorController(robotID, physicsClient,
timeStep, motor_kp, motor_kd, motor_torque, motor_max_velocity, jd)
jointNameToId, jointNum_revolute, jointId_list = motor_c.getRevoluteJointIndex()
# disable default constraint-based motors
for i in range(jointNum_revolute):
p.setJointMotorControl2(
robotID, jointId_list[i], p.POSITION_CONTROL, targetPosition=0, force=0)
# ----------------------------
# set motor of each joint
# ----------------------------
# --> body
# motor_hip = jointNameToId['joint_baseLink_to_hip']
planeId = p.loadURDF("plane.urdf")
humanoid = p.loadURDF("humanoid_leg_12dof.4.urdf", [0.5, 0, 0.05],
p.getQuaternionFromEuler([0, 0, 1.5707963]),
useFixedBase=False)
nJoints = p.getNumJoints(humanoid)
print(nJoints)
jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(humanoid, i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
motorsController = motorController.MotorController(humanoid, motor_kp,
motor_kd, motor_torque,
motor_max_velocity,
fixedTimeStep)
motorsController.printMotorsAngle()
walk = walkGenerator.WalkGenerator()
walk.setWalkParameter(bodyMovePoint=8,
legMovePoint=8,
h=50,
l=90,
sit=40,
swayBody=45,
swayFoot=0,
bodyPositionXPlus=5,
swayShift=3,
weightStart=0.4,
weightEnd=0.7,