Ejemplo n.º 1
0
def default_pos():
    for i in [1,2,3,4,5,6]:
            angles=kinematics.computeIK(i,180,0,90)
            print("Initialisatin des pattes: ")
            ids=[i*10+1, i*10+2, i*10+3]
            dxl_io.set_goal_position({ids[0]:angles[0] , ids[1]:angles[1] ,ids[2]:angles[2]})
            print("Patte",i,"initialisee")
Ejemplo n.º 2
0
def set_leg_pos(leg_id, x, y, z):
    id1 = leg_id * 10 + 1
    id2 = leg_id * 10 + 2
    id3 = leg_id * 10 + 3
    angles = kinematics.computeIK(x, y, z)
    ports = pypot.dynamixel.get_available_ports()
    dxl_io = pypot.dynamixel.DxlIO(ports[0], baudrate=1000000)
    dxl_io.set_goal_position({id1: angles[0], id2: angles[1], id3: angles[2]})
Ejemplo n.º 3
0
def set_leg_pos(leg_id, x, y, z):
    id1 = leg_id * 10 + 1
    id2 = leg_id * 10 + 2
    id3 = leg_id * 10 + 3
    angles = kinematics.computeIK(x, y, z)
    Initrobot.dxl_io.set_goal_position({
        id1: angles[0],
        id2: angles[1],
        id3: angles[2]
    })
Ejemplo n.º 4
0
def main():
    # ports = pypot.dynamixel.get_available_ports()
    # dxl_io = pypot.dynamixel.DxlIO(ports[0], baudrate=1000000)
    #
    # robot = SimpleRobot(dxl_io)
    # 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:

        params = Parameters(
            freq=50,
            speed=1,
            initz=-0.06,
            travelDistancePerStep=0.08,
            lateralDistance=0.18,
            frontDistance=0.087,
            frontStart=0.032,
            method="minJerk",
        )

        print("Setting initial position")
        # for k, v in robot.legs.items():
        for k in range(6):
            print("Goal:", params.initLeg[k])
            angle = kinematics.computeIK(params.initLeg[k][0],
                                         params.initLeg[k][1] + 0.05,
                                         params.initLeg[k][2])
            print("Angles:", angle)
            # for i in range(3):
            #     v[i].goal_position = angle[i]

        # TODO create this function instead:
        # setPositionToRobot(0, 0, 0, robot, params)
        # robot.smooth_tick_read_and_write(3, verbose=False)
        print("Init position reached")
        time.sleep(2)
        print("Closing")
    except Exception as e:
        track = traceback.format_exc()
        print(track)
Ejemplo n.º 5
0
def circle_demo(period, r):
    t0 = time.time()
    ports = pypot.dynamixel.get_available_ports()
    dxl_io = pypot.dynamixel.DxlIO(ports[0], baudrate=1000000)
    while True:
        t = time.time() - t0
        theta = 2 * math.pi * t / period
        z = 0
        x, y = circle(r, theta)
        x = x + 200
        # Il suffit d'envoyer ça à l'IK
        angles = kinematics.computeIK(x, y, z)
        dxl_io.set_goal_position({41: angles[0], 42: angles[1], 43: angles[2]})
        time.sleep(0.05)
Ejemplo n.º 6
0
                                     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)
                print(
                    "Asked IK for x:{}, y:{}, z{}, got theta1:{}, theta2:{}, theta3:{}"
                    .format(x, y, z, alphas[0], alphas[1], alphas[2]))
                targets = {
                    "motor1": -alphas[0],
                    "motor2": -alphas[1],
                    "motor3": alphas[2],
                }
            elif args.mode == "inverse-iterative":
                if (time.time() - lastInverse) > 0.1:
                    alphas = kinematics.inverseIterative(x, y, z)
                    targets = {
                        "motor1": -alphas[0],
                        "motor2": -alphas[1],
                        "motor3": alphas[2],
Ejemplo n.º 7
0
def moveLeg(legs, numleg, x, y, z):
	angles = kinematics.computeIK(x, y, z)
	pos = dict(zip(legs[numleg], angles))
	constant.dxl_io.set_goal_position(pos)
Ejemplo n.º 8
0
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])
Ejemplo n.º 9
0
def moveOnZ(coords, numleg, z):
	angles = kinematics.computeIK(coords[0], coords[1], coords[2]-z)
	pos = dict(zip(numleg, angles))
	constant.dxl_io.set_goal_position(pos)
Ejemplo n.º 10
0
def moveStraightOn(coords, numleg, x, y, z):
	angles = kinematics.computeIK(coords[0]+x, coords[1]+y, coords[2]+z)
	pos = dict(zip(numleg, angles))
	constant.dxl_io.set_goal_position(pos)
Ejemplo n.º 11
0
        # Mode simplement utilise pendant le développement
        keys = p.getKeyboardEvents()
        print(keys)

    elif args.mode == "direct":
        for name in controls.keys():
            targets[name] = p.readUserDebugParameter(controls[name])
            print("name:", name, ", valeur:", targets[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"])
        # 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]
Ejemplo n.º 12
0
import pypot.dynamixel
import numpy
import kinematics

#Analyse des ports
ports = pypot.dynamixel.get_available_ports()

#Creation de la connexion serie
with pypot.dynamixel.DxlIO(ports[0], baudrate=1000000) as dxl_io:
    #scan du robot
    angles = kinematics.computeIK(160, 0, 90)

    for i in [1, 2, 3, 4, 5, 6]:
        print("Initialisatin des pattes: ")
        ids = [i * 10 + 1, i * 10 + 2, i * 10 + 3]
        dxl_io.set_goal_position({
            ids[0]: angles[0],
            ids[1]: angles[1],
            ids[2]: angles[2]
        })
        print("Patte", i, "initialisee")
Ejemplo n.º 13
0
def set_leg_pos(leg_id,x,y,z):
    ids=[leg_id*10+1, leg_id*10+2, leg_id*10+3]
    angles=kinematics.computeIK(leg_id,x,y,z)  
    
    robot.dxl_io.set_goal_position({ids[0]:angles[0],ids[1]:angles[1],ids[2]:angles[2]})
Ejemplo n.º 14
0
                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"])
        z = p.readUserDebugParameter(controls["target_z"])
        # Use your own IK function
        alphas = kinematics.computeIK(x, y, z, verbose=True, 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(
Ejemplo n.º 15
0
import pypot.dynamixel
import numpy  
import kinematics

#Analyse des ports
ports=pypot.dynamixel.get_available_ports()

#Creation de la connexion serie
with pypot.dynamixel.DxlIO(ports[0], baudrate=1000000)  as dxl_io :
    #scan du robot
    #found_ids=dxl_io.scan()
    found_ids=[21,22,23]
    print(found_ids)
    print("DXL_IO= ",dxl_io)

    for i in [1,2,3,4,5,6]:
        angles=kinematics.computeIK(i,180,0,90)
        print("Initialisatin des pattes: ")
        ids=[i*10+1, i*10+2, i*10+3]
        dxl_io.set_goal_position({ids[0]:angles[0] , ids[1]:angles[1] ,ids[2]:angles[2]})
        print("Patte",i,"initialisee")
Ejemplo n.º 16
0
        # 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():
            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))
Ejemplo n.º 17
0
import pypot.dynamixel
import numpy
import kinematics

#Analyse des ports
ports = pypot.dynamixel.get_available_ports()

#Creation de la connexion serie
with pypot.dynamixel.DxlIO(ports[0], baudrate=1000000) as dxl_io:
    #scan du robot

    for i in [1, 2, 3, 4, 5, 6]:
        print("Initialisatin des pattes: ")
        angles = kinematics.computeIK(i, 160, -40, 90)
        ids = [i * 10 + 1, i * 10 + 2, i * 10 + 3]
        dxl_io.set_goal_position({
            ids[0]: angles[0],
            ids[1]: angles[1],
            ids[2]: angles[2]
        })
        print("Patte", i, "initialisee")
Ejemplo n.º 18
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"])
        # Use your own IK function
        alphas = kinematics.computeIK(x, y, z, 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]
        p.resetBasePositionAndOrientation(
            cross, T, to_pybullet_quaternion(0, 0, leg_angle))