def next_point(dx, dy, dz, mt):
ports = pypot.dynamixel.get_available_ports()
with pypot.dynamixel.DxlIO(ports[0], baudrate=1000000) as dxl_io:
global a
#Recuperation de la position en x,y,z d'une patte
for i in [1, 3, 5]:
ids = i
x = dxl_io.get_present_position([ids * 10 + 1])
y = dxl_io.get_present_position([ids * 10 + 2])
z = dxl_io.get_present_position([ids * 10 + 3])
x = x[0]
y = y[0]
z = z[0]
kinematics.computeDK(x, y, z)
#Ajout de la derivee a la position de la patte
#Exemple: ny= nouvelle valeur, y=valeur lue sur la patte, dy=valeur a ajouter
#mutliplication par sin(mt)
# Si mt=0--> nx=x+dx*math.sin(mt) => nx=x
nx = x + dx * math.sin(mt)
ny = y + dy * math.sin(mt)
nz = z + dz * math.sin(mt)
print("nx=", nx)
print("ny=", ny)
print("nz=", nz)
#Envoi des nouvelles positions pour le deplacement du robot
calcul.set_leg_pos_robot_frame(ids, nx, ny, nz)
def robotik():
last_points = None
for leg_id in range(6):
alphas = kinematics.computeIKOriented(
0.01 * math.sin(2 * math.pi * 0.5 * time.time() * 2),
0.02 * math.cos(2 * math.pi * 0.5 * time.time() * 2),
0.03 * math.sin(2 * math.pi * 0.2 * time.time() * 2),
leg_id,
params,
)
set_leg_angles(alphas, leg_id, targets, params)
leg = params.legs[0]
point = kinematics.computeDK(targets[leg[0]], targets[leg[1]],
targets[leg[2]])
if last_points is None:
last_points = time.time(), absolute_position(0, point,
sim.getRobotPose())
elif time.time() - last_points[0] > 0.1:
tip = absolute_position(0, point, sim.getRobotPose())
p.addUserDebugLine(last_points[1], tip, [1, 0, 0], 2., 10.)
last_points = time.time(), tip
# p.addUserDebugLine([0, 0, 0], getLegTip(), [1, 0, 0], 2., 10.)
#sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
state = sim.setJoints(targets)
def next_point(ids, dx, dy, dz, mt):
#Recuperation de la position en x,y,z d'une patte
x = dxl_io.get_present_position([ids * 10 + 1])
y = dxl_io.get_present_position([ids * 10 + 2])
z = dxl_io.get_present_position([ids * 10 + 3])
x = x[0]
y = y[0]
z = z[0]
kinematics.computeDK(x, y, z)
global a
#Ajout de la derivee a la position de la patte
if mt == 0:
if a == 0:
print("Boucle 1")
nx = x + dx * math.sin(mt)
ny = y + dy * math.sin(mt)
nz = z + dz
if y > -0.6 and y < 0.4:
a = 1
return a
if x > 154.7 and x < 155.3:
c = 1
return c
if a == 1 or c == 1:
ny = y + dy * math.sin(mt)
nz = z - dz
print("Boucle 2")
if y > 39.6 and y < 40.4:
a = 2
return a
if x > 170.7 and x < 171.3:
c = 2
return c
if a == 2:
ny = y - 2 * dy
print("Boucle 3")
if y > -40.6 and y < -39.8:
a = 0
return a
if c == 2:
nx = x - 2 * dx
if x > 139.7 and x < 1410.3:
c = 0
return c
#Envoi des nouvelles positions pour le deplacement du robot
calcul.set_leg_pos_robot_frame(ids, nx, ny, nz)
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 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()
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 = kinematics.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])
if args.mode == "inverse":
alphas = kinematics.computeIK(x, y, z)
def getCoordsToRotateOneLeg(coords, x, i): angles = kinematics.computeIK(coords[i][0], coords[i][1], coords[i][2]) angles[0] = angles[0] + x return kinematics.computeDK(angles[0], angles[1], angles[2])
def goUpOneLeg(legs, i): newAngles = constant.dxl_io.get_present_position(legs[i]) newCoords = kinematics.computeDK(newAngles[0], newAngles[1], newAngles[2]) moveStraightOn(newCoords, legs[i], 0, 0, constant.HEIGHT_TO_MOVE)
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
crosses = []
for i in range(4*6):
crosses.append(p.loadURDF("target2/robot.urdf"))
for name in sim.getJoints():
print(name)
if "c1" in name or "thigh" in name or "tibia" in name:
controls[name] = p.addUserDebugParameter(name, -math.pi, math.pi, 0)
elif args.mode == "direct":
for name in sim.getJoints():
print(name)
if "c1" in name or "thigh" in name or "tibia" in name:
controls[name] = p.addUserDebugParameter(name, -math.pi, math.pi, 0)
elif args.mode == "inverse":
cross = p.loadURDF("target2/robot.urdf")
# Use your own DK function
alphas = kinematics.computeDK(0, 0, 0, use_rads=True)
controls["target_x"] = p.addUserDebugParameter("target_x", -0.4, 0.4, alphas[0])
controls["target_y"] = p.addUserDebugParameter("target_y", -0.4, 0.4, alphas[1])
controls["target_z"] = p.addUserDebugParameter("target_z", -0.4, 0.4, alphas[2])
elif args.mode == "walk":
controls["teta"] = p.addUserDebugParameter("Direction", -math.pi, math.pi, 0)
controls["freq"] = p.addUserDebugParameter("Frequence", 0, 5, 1)
controls["length"] = p.addUserDebugParameter("Longueur", 0, 0.15, 0.1)
controls["height"] = p.addUserDebugParameter("Hauteur des pas", 0, 0.2, 0.07)
controls["body"] = p.addUserDebugParameter("Hauteur du corps", 0, 0.2, 0)
elif args.mode == "move-center":
controls["target_x"] = p.addUserDebugParameter("target_x", -0.3, 0.3, 0)
controls["target_y"] = p.addUserDebugParameter("target_y", -0.3, 0.3, 0)
controls["target_z"] = p.addUserDebugParameter("target_z", -0.3, 0.3, 0)
i = 0
for pt in points:
move_cross_attached(crosses[i], 0, pt, pos)
i += 1
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
state = sim.setJoints(targets)
elif args.mode == "direct":
for name in controls.keys():
targets[name] = p.readUserDebugParameter(controls[name])
points = []
for leg in params.legs.values():
points.append(
kinematics.computeDK(targets[leg[0]], targets[leg[1]],
targets[leg[2]]))
pos = sim.getRobotPose()
i = 0
for pt in points:
move_cross_attached(crosses[i], i, pt, pos)
i += 1
#sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
state = sim.setJoints(targets)
elif args.mode == "inverse":
adds = kinematics.computeDK(LEG_CENTER_POS[0][0], LEG_CENTER_POS[0][1],
LEG_CENTER_POS[0][2])
x = p.readUserDebugParameter(controls["target_x"]) + adds[0]
y = p.readUserDebugParameter(controls["target_y"]) + adds[1]
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():
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"])
z = p.readUserDebugParameter(controls["target_z"])
alphas = kinematics.computeIK(x, y, z, 0, verbose=False)
dk0 = kinematics.computeDK(0, 0, 0, use_rads=True)
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(
cross, T, to_pybullet_quaternion(0, 0, leg_angle))
if "c1" in name or "thigh" in name or "tibia" in name:
controls[name] = p.addUserDebugParameter(name, -math.pi, math.pi,
0)
elif args.mode == "robot-ik-keyboard":
x_body, y_body, z_body = 0, 0, params.z
max_value = 0.05
value = 0.001
elif args.mode == "inverse":
crosses = []
for i in range(5):
crosses.append(p.loadURDF("target2/robot.urdf"))
# Use your own DK function
alphas = kinematics.computeDK(
0, 0, 0, use_rads=True
) # [0, constants.THETA2_MOTOR_SIGN * constants.theta2Correction, constants.THETA3_MOTOR_SIGN * constants.theta3Correction]
controls["target_x"] = p.addUserDebugParameter("target_x", -0.4, 0.4,
alphas[0])
controls["target_y"] = p.addUserDebugParameter("target_y", -0.4, 0.4,
alphas[1])
controls["target_z"] = p.addUserDebugParameter("target_z", -0.4, 0.4,
alphas[2])
elif args.mode == "walk" or args.mode == "rotate":
last_angles = 18 * [0]
elif args.mode == "walk-configurable":
last_angles = 18 * [0]
controls["angle"] = p.addUserDebugParameter("angle", 0, 360, 0)
controls["step_dist"] = p.addUserDebugParameter("step distance", 0, 0.3, 0)
for name in sim.getJoints():
print(name)
if "c1" in name or "thigh" in name or "tibia" in name:
controls[name] = p.addUserDebugParameter(name, -math.pi, math.pi,
0)
elif args.mode == "direct":
for name in sim.getJoints():
print(name)
if "c1" in name or "thigh" in name or "tibia" in name:
controls[name] = p.addUserDebugParameter(name, -math.pi, math.pi,
0)
elif args.mode == "inverse":
cross = p.loadURDF("target2/robot.urdf")
alphas = kinematics.computeDK(0, 0, 0, use_rads=True)
controls["target_x"] = p.addUserDebugParameter("target_x", -0.4, 0.4,
alphas[0])
controls["target_y"] = p.addUserDebugParameter("target_y", -0.4, 0.4,
alphas[1])
controls["target_z"] = p.addUserDebugParameter("target_z", -0.4, 0.4,
alphas[2])
elif args.mode == "robot-ik":
controls["target_x"] = p.addUserDebugParameter("target_x", -0.1, 0.1)
controls["target_y"] = p.addUserDebugParameter("target_y", -0.1, 0.1)
controls["target_z"] = p.addUserDebugParameter("target_z", -0.1, 0.1)
elif args.mode == "rotation":
controls["target_x"] = p.addUserDebugParameter("target_x", 0, 0.3, 0.180)
controls["target_z"] = p.addUserDebugParameter("target_z", -1, 0.5, -0.15)