def walk(speed_x, speed_y, speed_rot, height, max_ampl, z):
speed_abs = abs(speed_x) + abs(speed_y) + abs(speed_rot)
duree = (1 / speed_abs if speed_abs != 0 else 0)
x = 0
y = 0
rot = 0
if speed_abs > max_ampl:
diff = speed_abs - max_ampl
x = speed_x - diff * speed_x / speed_abs
y = speed_y - diff * speed_y / speed_abs
rot = speed_rot - diff * speed_rot / speed_abs
t = time.time()
Linear = kinematics.LinearStep(x, y, z, height, duree)
Rotation = kinematics.RotationStep(
[-params.initLeg[0], -params.initLeg[1], 0], rot * 2 * math.pi, duree)
for leg_id in range(6):
pos = Linear.step(t, leg_id % 2)
pos2 = kinematics.rotation_z(Rotation.step(t, leg_id % 2),
LEG_ANGLES[leg_id])
alphas = kinematics.computeIKOriented(
pos[0] + pos2[0],
pos[1] + pos2[1],
pos[2] + pos2[2],
leg_id,
params,
)
set_leg_angles(alphas, leg_id, targets, params)
state = sim.setJoints(targets)
def egg_twerk(sim, targets, frozen=0):
global params
new_t = math.fmod(sim.t, 2)
x = 0
y = 0
z = 0
for legID in range(0, 6):
if legID == 0 or legID == 1:
z = 0.05
elif legID == 2 or legID == 5:
z = 0.05
elif legID == 3 or legID == 4:
z = 0.05 - abs(0.06 * math.sin(math.pi * new_t * 4))
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)
sim.lookAt(robot_position[0])
if frozen == 1:
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
sim.tick()
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 attack(params):
"""
Idle position.
Its body moves with a little sinus and its front legs wave as if it was attacking someone
"""
angles = []
t = time.time()
for leg_id in range(1, 7):
if leg_id == 1 or leg_id == 2:
angles.append([
0.1 * math.fabs(math.tan(t)), 0,
0.1 + 0.2 * math.fabs(math.cos(t))
])
elif leg_id == 3 or leg_id == 6:
angles.append([0.1, 0.01 * math.cos(t), 0.01 * math.cos(t)])
else:
angles.append([0.01 * math.cos(t), 0.01 * math.cos(t), 0.01])
for leg_id in range(1, 7):
angle = angles[leg_id - 1]
alphas = kinematics.computeIKOriented(
angle[0],
angle[1],
angle[2],
leg_id,
params,
verbose=False,
)
set_leg_angles(alphas, leg_id, targets, params)
state = sim.setJoints(targets)
sim.tick()
def initRobot(params):
targets = {}
for leg_id in range(1, 7):
alphas = kinematics.computeIKOriented(0, 0, 0, leg_id, params)
set_leg_angles(alphas, leg_id, targets, params)
return alphas
state = sim.setJoints(targets)
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 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 mouseRobot(params, coef=1, verbose=False, z=0):
mp = getMousePosition()
hasMoved = didItMoved(mp, z)
mouse_x, mouse_y = mappingPad(mp[0], 0, 0.1, 0.0, 0.01), mappingPad(mp[1], 0, 0.1, 0, 0.01)
if verbose and hasMoved: print(
"Mouse in pos ({}, {}) gives: [{:.2f}, {:.2f}] (z={:.2f})".format(mp[0], mp[1], mouse_x, mouse_y, z))
# Use your own IK function
alphas_list = []
for leg_id in range(1, 7):
# alphas = kinematics.computeIK(mouse_x, mouse_y, z, verbose=True, use_rads=False)
alphas = kinematics.computeIKOriented(
mouse_x,
mouse_y,
z,
leg_id,
params,
verbose=False,
)
alphas_list.append(alphas)
if verbose and hasMoved:
print(alphas_list)
return alphas_list, hasMoved
def main():
ports = pypot.dynamixel.get_available_ports()
dxl_io = pypot.dynamixel.DxlIO(ports[0], baudrate=1000000)
robot = SimpleRobot(dxl_io)
parser = argparse.ArgumentParser()
parser.add_argument("--mode",
"-m",
type=str,
default="direct",
help="test")
args = parser.parse_args()
robot.init()
time.sleep(0.1)
robot.enable_torque()
# Defining the shutdown function here so it has visibility over the robot variable
def shutdown(signal_received, frame):
# Handle any cleanup here
print(
"SIGINT or CTRL-C detected. Setting motors to compliant and exiting"
)
robot.disable_torque()
print("Done ticking. Exiting.")
sys.exit()
# Brutal exit
# os._exit(1)
# Tell Python to run the shutdown() function when SIGINT is recieved
signal(SIGINT, shutdown)
try:
print("Setting initial position")
for k, v in robot.legs.items():
v[0].goal_position = read_init(init, int(10 * k + 1))
v[1].goal_position = read_init(init, int(10 * k + 2))
v[2].goal_position = read_init(init, int(10 * k + 3))
robot.smooth_tick_read_and_write(3, verbose=False)
print("Init position reached")
if args.mode == "mouse":
verboseMain = False
while True:
alphas, hasMoved = mouse.mouseRobot(params=robot.params,
coef=20,
verbose=True,
z=0.01) #input("z?"))
moveFromAlphas(robot, alphas, hasMoved, verboseMain=False)
time.sleep(2)
print("Closing")
elif args.mode == "sinus":
while True:
t = time.time()
for leg_id in range(1, 7):
angle = [0.05 * math.sin(t), 0.05 * math.cos(t), -0.01]
alphas = kinematics.computeIKOriented(
angle[0],
angle[1],
angle[2],
leg_id,
robot.params,
verbose=False,
)
v = robot.legs[leg_id]
v[0].goal_position = alphas[0] * 10
v[1].goal_position = alphas[1] * 10
v[2].goal_position = alphas[2] * 10
print(v)
time.sleep(1 / 1000)
robot.smooth_tick_read_and_write(0.5, verbose=False)
except Exception as e:
track = traceback.format_exc()
print(track)
finally:
shutdown(None, None)
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)
)
elif args.mode == "robot-ik":
None
# Use your own IK function
#sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
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,
params,
verbose=True,
)
# alphas = kinematics.computeIKOriented(
# 0.01 * math.sin(2 * math.pi * 0.5 * time.time()),
# 0*0.02 * math.cos(2 * math.pi * 0.5 * time.time()),
# 0*0.03 * math.sin(2 * math.pi * 0.2 * time.time()),
# leg_id,
# params,
# verbose=True,
# )
set_leg_angles(alphas, leg_id, targets, params)
state = sim.setJoints(targets)
elif args.mode == "walk":
None
targets["j_c1_rf"] = alphas[0]
targets["j_thigh_rf"] = alphas[1]
targets["j_tibia_rf"] = alphas[2]
move_cross_attached(cross, 0, [x, y, z], sim.getRobotPose())
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
state = sim.setJoints(targets)
elif args.mode == "robot-ik":
# Use your own IK function
for leg_id in range(6):
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,
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.)
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,
params,
verbose=False,
)
set_leg_angles(alphas, leg_id, targets, params)
# sim.setRobotPose(
# [0, 0, 0.5],
# to_pybullet_quaternion(0, 0, 0),
# )
state = sim.setJoints(targets)
elif args.mode == "robot-ik-keyboard":
keys = p.getKeyboardEvents()
if 122 in keys:
x_body = min(x_body + value, max_value)
if 115 in keys:
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))
elif args.mode == "robot-ik":
x = p.readUserDebugParameter(controls["target_x"])
y = p.readUserDebugParameter(controls["target_y"])
z = p.readUserDebugParameter(controls["target_z"])
for leg_id in range(1, 7):
# To create movement : A * math.sin(2 * math.pi * 0.5 * time.time())
# with A as amplitude (x, y, z, like 0.03m, or the parameters above)
alphas = kinematics.computeIKOriented(x, y, z, leg_id, params)
set_leg_angles(alphas, leg_id, targets, params)
state = sim.setJoints(targets)
elif args.mode == "rotation":
x = p.readUserDebugParameter(controls["target_x"])
z = p.readUserDebugParameter(controls["target_z"])
h = p.readUserDebugParameter(controls["target_h"])
w = p.readUserDebugParameter(controls["target_w"])
period = p.readUserDebugParameter(controls["period"])
for leg_id in range(1, 7):
if (leg_id == 1) or (leg_id == 3) or (leg_id == 5):
alphas = kinematics.triangle_for_rotation(
x, z, h, w, sim.t, period)
set_leg_angles(alphas, leg_id, targets, params)