def move_legs(sim,
targets,
legID=0,
leg_target_x=0,
leg_target_y=0,
leg_target_z=0,
debug=False,
frozen=0):
global params
alphas = kinematics.computeIKOriented(leg_target_x, leg_target_y,
leg_target_z, legID)
targets[params.legs[legID][0]] = alphas[0]
targets[params.legs[legID][1]] = alphas[1]
targets[params.legs[legID][2]] = alphas[2]
robot_position = sim.getRobotPose()
state = sim.setJoints(targets)
if debug:
pos = kinematics.computeDK(state[params.legs[legID][0]][0],
state[params.legs[legID][1]][0],
state[params.legs[legID][2]][0])
pos = kinematics.rotaton_2D(pos[0], pos[1], pos[2],
LEG_ANGLES[legID] + robot_position[1][2])
new_centers = kinematics.rotaton_2D(LEG_CENTER_POS[legID][0],
LEG_CENTER_POS[legID][1],
LEG_CENTER_POS[legID][2],
robot_position[1][2])
pos[0] += new_centers[0] + robot_position[0][0]
pos[1] += new_centers[1] + robot_position[0][1]
pos[2] += new_centers[2] + robot_position[0][2]
sim.addDebugPosition(pos, duration=1.5)
sim.lookAt(robot_position[0])
if frozen == 1:
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
sim.tick()
def egg_wave(sim, targets, legID=0, debug=False, frozen=0):
global params
new_t = math.fmod(sim.t, 2)
x = 0.1
y = abs(0.1 * math.sin(math.pi * new_t))
z = 0.2
alphas = kinematics.computeIKOriented(x, y, z, legID)
targets[params.legs[legID][0]] = alphas[0]
targets[params.legs[legID][1]] = alphas[1]
targets[params.legs[legID][2]] = alphas[2]
robot_position = sim.getRobotPose()
state = sim.setJoints(targets)
if debug:
if 0 < math.fmod(sim.t, 0.05) < 0.02:
pos = kinematics.computeDK(state[params.legs[legID][0]][0],
state[params.legs[legID][1]][0],
state[params.legs[legID][2]][0])
pos = kinematics.rotaton_2D(
pos[0], pos[1], pos[2],
LEG_ANGLES[legID] + robot_position[1][2])
new_centers = kinematics.rotaton_2D(LEG_CENTER_POS[legID][0],
LEG_CENTER_POS[legID][1],
LEG_CENTER_POS[legID][2],
robot_position[1][2])
pos[0] += new_centers[0] + robot_position[0][0]
pos[1] += new_centers[1] + robot_position[0][1]
pos[2] += new_centers[2] + robot_position[0][2]
sim.addDebugPosition(pos, duration=1.5)
sim.lookAt(robot_position[0])
if frozen == 1:
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
sim.tick()
def print_points(points):
i = -1
T = []
print("#####")
for pt in points:
# Drawing each step of the DK calculation
i += 1
T.append(kinematics.rotaton_2D(pt[0], pt[1], pt[2], leg_angle))
T[-1][0] += leg_center_pos[0] # Ajout l'offset de l'épaule
T[-1][1] += leg_center_pos[1]
T[-1][2] += leg_center_pos[2]
print("Drawing cross {} at {}".format(i, T[-1]))
p.resetBasePositionAndOrientation(
crosses[i], T[-1], to_pybullet_quaternion(0, 0, leg_angle))
def move_body(sim,
targets,
body_target_x=0,
body_target_y=0,
body_target_z=0,
debug=False,
frozen=0):
alphas = kinematics.spider(-body_target_x, -body_target_y, -body_target_z)
targets["j_c1_rf"] = alphas[0][0]
targets["j_thigh_rf"] = alphas[0][1]
targets["j_tibia_rf"] = alphas[0][2]
targets["j_c1_lf"] = alphas[1][0]
targets["j_thigh_lf"] = alphas[1][1]
targets["j_tibia_lf"] = alphas[1][2]
targets["j_c1_lm"] = alphas[2][0]
targets["j_thigh_lm"] = alphas[2][1]
targets["j_tibia_lm"] = alphas[2][2]
targets["j_c1_lr"] = alphas[3][0]
targets["j_thigh_lr"] = alphas[3][1]
targets["j_tibia_lr"] = alphas[3][2]
targets["j_c1_rr"] = alphas[4][0]
targets["j_thigh_rr"] = alphas[4][1]
targets["j_tibia_rr"] = alphas[4][2]
targets["j_c1_rm"] = alphas[5][0]
targets["j_thigh_rm"] = alphas[5][1]
targets["j_tibia_rm"] = alphas[5][2]
robot_position = sim.getRobotPose()
sim.lookAt(robot_position[0])
state = sim.setJoints(targets)
if debug:
for leg_id in range(0, 6):
pos = kinematics.computeDK(state[params.legs[leg_id][0]][0],
state[params.legs[leg_id][1]][0],
state[params.legs[leg_id][2]][0])
pos = kinematics.rotaton_2D(
pos[0], pos[1], pos[2],
LEG_ANGLES[leg_id] + robot_position[1][2])
new_centers = kinematics.rotaton_2D(LEG_CENTER_POS[leg_id][0],
LEG_CENTER_POS[leg_id][1],
LEG_CENTER_POS[leg_id][2],
robot_position[1][2])
pos[0] += new_centers[0] + robot_position[0][0]
pos[1] += new_centers[1] + robot_position[0][1]
pos[2] += new_centers[2] + robot_position[0][2]
sim.addDebugPosition(pos, duration=1.5)
if frozen == 1:
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
sim.tick()
def rotate(sim,
targets,
speed=1,
direction=0,
robot_height=0,
step_height=0,
debug=False,
frozen=0):
alphas = kinematics.rotate(sim.t,
duration=speed,
direction=direction,
robot_height=robot_height,
step_height=step_height)
targets["j_c1_rf"] = alphas[0][0]
targets["j_thigh_rf"] = alphas[0][1]
targets["j_tibia_rf"] = alphas[0][2]
targets["j_c1_lf"] = alphas[1][0]
targets["j_thigh_lf"] = alphas[1][1]
targets["j_tibia_lf"] = alphas[1][2]
targets["j_c1_lm"] = alphas[2][0]
targets["j_thigh_lm"] = alphas[2][1]
targets["j_tibia_lm"] = alphas[2][2]
targets["j_c1_lr"] = alphas[3][0]
targets["j_thigh_lr"] = alphas[3][1]
targets["j_tibia_lr"] = alphas[3][2]
targets["j_c1_rr"] = alphas[4][0]
targets["j_thigh_rr"] = alphas[4][1]
targets["j_tibia_rr"] = alphas[4][2]
targets["j_c1_rm"] = alphas[5][0]
targets["j_thigh_rm"] = alphas[5][1]
targets["j_tibia_rm"] = alphas[5][2]
robot_position = sim.getRobotPose()
state = sim.setJoints(targets)
if debug:
for leg_id in range(0, 6):
pos = kinematics.computeDK(state[params.legs[leg_id][0]][0],
state[params.legs[leg_id][1]][0],
state[params.legs[leg_id][2]][0])
pos = kinematics.rotaton_2D(
pos[0], pos[1], pos[2],
LEG_ANGLES[leg_id] + robot_position[1][2])
new_centers = kinematics.rotaton_2D(LEG_CENTER_POS[leg_id][0],
LEG_CENTER_POS[leg_id][1],
LEG_CENTER_POS[leg_id][2],
robot_position[1][2])
pos[0] += new_centers[0] + robot_position[0][0]
pos[1] += new_centers[1] + robot_position[0][1]
pos[2] += new_centers[2] + robot_position[0][2]
sim.addDebugPosition(pos, duration=1.5)
if frozen == 1:
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
sim.tick()
def walk(sim,
targets,
speed=1,
rotation_speed=0,
direction=0,
robot_height=0.05,
step_height=0.01,
step_lenght=0.15,
debug=False,
frozen=0,
send_drift=False,
legs_offset=0.02,
walk_mode='triangle',
rotate=0):
global params
global old_distances
global old_linear_robot_pos
drift = False
alphas = kinematics.walk(sim.t,
duration=speed,
direction=direction,
robot_height=robot_height,
step_height=step_height,
step_lenght=step_lenght,
legs_offset=legs_offset,
rotate=rotate)
targets["j_c1_rf"] = alphas[0][0]
targets["j_thigh_rf"] = alphas[0][1]
targets["j_tibia_rf"] = alphas[0][2]
targets["j_c1_lf"] = alphas[1][0]
targets["j_thigh_lf"] = alphas[1][1]
targets["j_tibia_lf"] = alphas[1][2]
targets["j_c1_lm"] = alphas[2][0]
targets["j_thigh_lm"] = alphas[2][1]
targets["j_tibia_lm"] = alphas[2][2]
targets["j_c1_lr"] = alphas[3][0]
targets["j_thigh_lr"] = alphas[3][1]
targets["j_tibia_lr"] = alphas[3][2]
targets["j_c1_rr"] = alphas[4][0]
targets["j_thigh_rr"] = alphas[4][1]
targets["j_tibia_rr"] = alphas[4][2]
targets["j_c1_rm"] = alphas[5][0]
targets["j_thigh_rm"] = alphas[5][1]
targets["j_tibia_rm"] = alphas[5][2]
robot_position = sim.getRobotPose()
current_linear_robot_pos = [robot_position[0], sim.t]
lin_speeds = calc_speeds(current_linear_robot_pos, old_linear_robot_pos)
old_linear_robot_pos = [robot_position[0], sim.t]
sim.lookAt(robot_position[0])
state = sim.setJoints(targets)
if debug:
for leg_id in range(0, 6):
pos = kinematics.computeDK(state[params.legs[leg_id][0]][0],
state[params.legs[leg_id][1]][0],
state[params.legs[leg_id][2]][0])
pos = kinematics.rotaton_2D(
pos[0], pos[1], pos[2],
LEG_ANGLES[leg_id] + robot_position[1][2])
new_centers = kinematics.rotaton_2D(LEG_CENTER_POS[leg_id][0],
LEG_CENTER_POS[leg_id][1],
LEG_CENTER_POS[leg_id][2],
robot_position[1][2])
pos[0] += new_centers[0] + robot_position[0][0]
pos[1] += new_centers[1] + robot_position[0][1]
pos[2] += new_centers[2] + robot_position[0][2]
sim.addDebugPosition(pos, duration=1.5)
if frozen == 1:
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
if send_drift:
positions = []
for leg_id in range(0, 6):
pos = kinematics.computeDK(state[params.legs[leg_id][0]][0],
state[params.legs[leg_id][1]][0],
state[params.legs[leg_id][2]][0])
pos = kinematics.rotaton_2D(
pos[0], pos[1], pos[2],
LEG_ANGLES[leg_id] + robot_position[1][2])
new_centers = kinematics.rotaton_2D(LEG_CENTER_POS[leg_id][0],
LEG_CENTER_POS[leg_id][1],
LEG_CENTER_POS[leg_id][2],
robot_position[1][2])
pos[0] += new_centers[0] + robot_position[0][0]
pos[1] += new_centers[1] + robot_position[0][1]
pos[2] += new_centers[2] + robot_position[0][2]
positions.append(pos)
distances = []
distances.insert(0, distance_leg(positions[0], positions[2]))
distances.insert(1, distance_leg(positions[2], positions[4]))
distances.insert(2, distance_leg(positions[4], positions[0]))
distances.insert(3, distance_leg(positions[1], positions[3]))
distances.insert(4, distance_leg(positions[3], positions[5]))
distances.insert(5, distance_leg(positions[5], positions[1]))
for i in range(0, 6):
for j in range(0, 3):
if abs(distances[i][j] - old_distances[i][j]) > 0.001:
drift = True
old_distances[i][j] = distances[i][j]
sim.tick()
return drift, lin_speeds
if "c1" in name or "thigh" in name or "tibia" in name:
targets[name] = 0
###############################################################################
if args.mode == "frozen-direct":
for name in controls.keys():
targets[name] = p.readUserDebugParameter(controls[name])
pts, T, steps, num_legs = [], [], 4, 6
for i in range(num_legs):
pts.extend(kinematics.computeDKDetailed(targets[params.legs[i+1][0]],
targets[params.legs[i+1][1]],
targets[params.legs[i+1][2]],
use_rads=True))
for pt in range(steps):
T.append(kinematics.rotaton_2D( pts[i*4+pt][0], pts[i*4+pt][1],
pts[i*4+pt][2], -LEG_ANGLES[i]))
T[-1] = list(map( lambda a,b,c : a+b+c, T[-1], LEG_CENTER_POS[i],
[0., 0., 0.5]))
p.resetBasePositionAndOrientation(crosses[i*4+pt], T[-1],
to_pybullet_quaternion(0, 0, -LEG_ANGLES[i]))
sim.setRobotPose([0,0,0.5], to_pybullet_quaternion(0, 0, 0))
state = sim.setJoints(targets)
###############################################################################
elif args.mode == "direct":
for name in controls.keys():
targets[name] = p.readUserDebugParameter(controls[name])
state = sim.setJoints(targets)
elif args.mode == "inverse":
x = p.readUserDebugParameter(controls["target_x"])
y = p.readUserDebugParameter(controls["target_y"])
targets[name] = 0
if args.mode == "frozen-direct":
for name in controls.keys():
targets[name] = p.readUserDebugParameter(controls[name])
points = kinematics.computeDKDetailed(
targets["j_c1_rf"],
targets["j_thigh_rf"],
targets["j_tibia_rf"],
use_rads=True,
)
i = -1
T = []
for pt in points:
# Drawing each step of the DK calculation
i += 1
T.append(kinematics.rotaton_2D(pt[0], pt[1], pt[2], leg_angle))
T[-1][0] += leg_center_pos[0]
T[-1][1] += leg_center_pos[1]
T[-1][2] += leg_center_pos[2]
# print("Drawing cross {} at {}".format(i, T))
p.resetBasePositionAndOrientation(
crosses[i], T[-1], to_pybullet_quaternion(0, 0, leg_angle))
# Temp
sim.setRobotPose([0, 0, 0.5], to_pybullet_quaternion(0, 0, 0))
# sim.setRobotPose(
# leg_center_pos, to_pybullet_quaternion(0, 0, 0),
# )
state = sim.setJoints(targets)
elif args.mode == "direct":
for name in controls.keys():
z = p.readUserDebugParameter(controls["target_z"])
# Use your own IK function
alphas = kinematics.computeIK(x, y, z, verbose=True, use_rads=True)
points = kinematics.computeDKDetailed(alphas[0], alphas[1], alphas[2])
print_points(points)
targets["j_c1_rf"] = alphas[0]
targets["j_thigh_rf"] = alphas[1]
targets["j_tibia_rf"] = alphas[2]
state = sim.setJoints(targets)
# Temp
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
T = kinematics.rotaton_2D(x, y, z, leg_angle)
T[0] += leg_center_pos[0]
T[1] += leg_center_pos[1]
T[2] += leg_center_pos[2]
# print("Drawing cross {} at {}".format(i, T))
p.resetBasePositionAndOrientation(
crosses[-1], T, to_pybullet_quaternion(0, 0, leg_angle))
elif args.mode == "robot-ik":
# Use your own IK function
for leg_id in range(1, 7):
alphas = kinematics.computeIKOriented(
0.01 * math.sin(2 * math.pi * 0.5 * time.time()),
0.02 * math.cos(2 * math.pi * 0.5 * time.time()),
0.03 * math.sin(2 * math.pi * 0.2 * time.time()),
leg_id,
if "c1" in name or "thigh" in name or "tibia" in name:
targets[name] = 0
if args.mode == "frozen-direct":
for name in controls.keys():
targets[name] = p.readUserDebugParameter(controls[name])
# Use your own DK function, the result should be: https://www.youtube.com/watch?v=w3psAbh3AoM
points = kinematics.computeDKDetailed(targets["j_c1_rf"],
targets["j_thigh_rf"],
targets["j_tibia_rf"],
use_rads=True)
i = -1
T = []
for pt in points:
# Drawing each step of the DK calculation
i += 1
T.append(kinematics.rotaton_2D(pt[0], pt[1], pt[2], leg_angle))
T[-1][0] += leg_center_pos[0]
T[-1][1] += leg_center_pos[1]
T[-1][2] += leg_center_pos[2]
# print("Drawing cross {} at {}".format(i, T))
p.resetBasePositionAndOrientation(
crosses[i], T[-1], to_pybullet_quaternion(0, 0, leg_angle))
sim.setRobotPose(
leg_center_pos,
to_pybullet_quaternion(0, 0, 0),
)
state = sim.setJoints(targets)
elif args.mode == "direct":
for name in controls.keys():
targets[name] = p.readUserDebugParameter(controls[name])
targets[name] = 0
if args.mode == "frozen-direct":
for name in controls.keys():
targets[name] = p.readUserDebugParameter(controls[name])
points = kinematics.computeDKDetailed(
targets["j_c1_rf"],
targets["j_thigh_rf"],
targets["j_tibia_rf"],
use_rads=True,
)
i = -1
T = []
for pt in points:
# Drawing each step of the DK calculation
i += 1
T.append(kinematics.rotaton_2D(pt[0], pt[1], pt[2], leg_angle))
T[-1][0] += leg_center_pos[0]
T[-1][1] += leg_center_pos[1]
T[-1][2] += leg_center_pos[2]
# print("Drawing cross {} at {}".format(i, T))
p.resetBasePositionAndOrientation(
crosses[i], T[-1], to_pybullet_quaternion(0, 0, leg_angle))
# Temp
sim.setRobotPose([0, 0, 0.5], to_pybullet_quaternion(0, 0, 0))
# sim.setRobotPose(
# leg_center_pos, to_pybullet_quaternion(0, 0, 0),
# )
state = sim.setJoints(targets)
elif args.mode == "direct":
