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)