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")
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]})
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]
})
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)
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)
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],
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)
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 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)
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)
# 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]
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")
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]})
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(
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")
# 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))
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")
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))
