def get_angles(xyz):
x = xyz[0]
y = xyz[1]
z = xyz[2]
my_chain = ikpy.chain.Chain.from_urdf_file("robot_2link.URDF")
target_vector = [x, y, z]
target_frame = np.eye(4)
target_frame[:3, 3] = target_vector
#print("The angles of each joints are : ", my_chain.inverse_kinematics(target_frame))
real_frame = my_chain.forward_kinematics(
my_chain.inverse_kinematics(target_frame))
#print("Computed position vector : %s, original position vector : %s" % (real_frame[:3, 3], target_frame[:3, 3]))
ax = plot_utils.init_3d_figure()
my_chain.plot([0, 0, 0, 0, 0], ax, target=target_vector)
my_chain.plot(my_chain.inverse_kinematics(target_frame),
ax,
target=target_vector)
angles = my_chain.inverse_kinematics(target_frame)
angles[1] = (angles[1] * 180) / 3.14
angles[2] = (angles[2] * 180) / 3.14
angles[3] = (angles[3] * 180) / 3.14
#print(angles)
#plt.xlim(-1,1)
#plt.ylim(-1,1)
#plt.show()
return angles
def get_angles():
my_chain = ikpy.chain.Chain.from_urdf_file("robot_2link.URDF")
target_vector = [0.6, 0.4, 0.3]
target_frame = np.eye(4)
target_frame[:3, 3] = target_vector
print("The angles of each joints are : ",
my_chain.inverse_kinematics(target_frame))
real_frame = my_chain.forward_kinematics(
my_chain.inverse_kinematics(target_frame))
print("Computed position vector : %s, original position vector : %s" %
(real_frame[:3, 3], target_frame[:3, 3]))
import matplotlib.pyplot as plt
ax = plot_utils.init_3d_figure()
my_chain.plot([0, 0, 0, 0, 0], ax, target=target_vector)
my_chain.plot(my_chain.inverse_kinematics(target_frame),
ax,
target=target_vector)
angles = my_chain.inverse_kinematics(target_frame)
#plt.xlim(-1,1)
#plt.ylim(-1,1)
#plt.show()
return angles
def test_ergo(resources_path, interactive):
a = chain.Chain.from_urdf_file(resources_path + "/testbot.urdf",
base_elements=["base"])
print(a)
target = [0.15, 0.15, 0.1]
#target = [0.07, 0.0, 0.0]
frame_target = np.eye(4)
frame_target[:3, 3] = target
joints = [0] * len(a.links)
print(joints)
ik = a.inverse_kinematics(frame_target, initial_position=joints)
print("The angles of each joints are : ",
a.inverse_kinematics(frame_target))
arr = a.inverse_kinematics(frame_target)
print("Angle 0: ", math.degrees(arr[0]))
print("Angle 1: ", math.degrees(arr[1]))
print("Angle 2: ", math.degrees(arr[2]))
print("Angle 3: ", math.degrees(arr[3]))
print("Angle 4: ", math.degrees(arr[4]))
brr = a.forward_kinematics(arr)
print("Computed position vector: ", brr[:3, 3])
ax = plot_utils.init_3d_figure()
a.plot(ik, ax, target=target)
plt.savefig("out/ergo.png")
if interactive:
plot_utils.show_figure()
def setUp(self):
if plot:
self.ax = plot_utils.init_3d_figure()
self.chain1 = chain.Chain.from_urdf_file(params.resources_path + "/poppy_torso.URDF", base_elements=["base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x", "chest", "r_shoulder_y"], last_link_vector=[0, 0.18, 0], active_links_mask=[False, False, False, False, True, True, True, True, True])
self.joints = [0] * len(self.chain1.links)
self.joints[-4] = 0
self.target = [0.1, -0.2, 0.1]
self.frame_target = np.eye(4)
self.frame_target[:3, 3] = self.target
def IK(target_frame):
ax = plot_utils.init_3d_figure()
my_chain = ikpy.chain.Chain.from_urdf_file("urdf/01-myfirst.urdf")
print(my_chain)
jnts = my_chain.inverse_kinematics(target_frame)
my_chain.plot(jnts, ax, target=target_frame[:3, 3])
plt.show()
return jnts
def test():
my_chain = ikpy.chain.Chain.from_urdf_file("poppy_ergo.URDF")
target_vector = [0.1, -0.2, 0.1]
target_frame = np.eye(4)
target_frame[:3, 3] = target_vector
print("The angles of each joints are : ",
my_chain.inverse_kinematics(target_frame))
ax = plot_utils.init_3d_figure()
my_chain.plot(my_chain.inverse_kinematics(target_frame),
ax,
target=target_vector,
show=True)
plt.xlim(-10, 10)
plt.ylim(-10, 10)
def test_ergo(resources_path, interactive):
a = chain.Chain.from_urdf_file(resources_path + "/poppy_ergo.URDF")
target = [0.1, -0.2, 0.1]
frame_target = np.eye(4)
frame_target[:3, 3] = target
joints = [0] * len(a.links)
ik = a.inverse_kinematics(frame_target, initial_position=joints)
ax = plot_utils.init_3d_figure()
a.plot(ik, ax, target=target)
plt.savefig("out/ergo.png")
if interactive:
plot_utils.show_figure()
def test_ergo(self):
a = chain.Chain.from_urdf_file(params.resources_path + "/poppy_ergo.URDF")
target = [0.1, -0.2, 0.1]
frame_target = np.eye(4)
frame_target[:3, 3] = target
joints = [0] * len(a.links)
ik = a.inverse_kinematics(frame_target, initial_position=joints)
if plot:
ax = plot_utils.init_3d_figure()
if plot:
a.plot(ik, ax, target=target)
plot_utils.show_figure()
def test_ergo(self):
a = chain.Chain.from_urdf_file(params.resources_path +
"/poppy_ergo.URDF")
target = [0.1, -0.2, 0.1]
frame_target = np.eye(4)
frame_target[:3, 3] = target
joints = [0] * len(a.links)
ik = a.inverse_kinematics(frame_target, initial_position=joints)
if plot:
ax = plot_utils.init_3d_figure()
if plot:
a.plot(ik, ax, target=target)
plot_utils.show_figure()
def setUp(self):
self.ax = plot_utils.init_3d_figure()
self.chain1 = chain.Chain.from_urdf_file(
params.resources_path + "/poppy_torso.URDF",
base_elements=[
"base", "abs_z", "spine", "bust_y", "bust_motors", "bust_x",
"chest", "r_shoulder_y"
],
last_link_vector=[0, 0.18, 0],
active_links_mask=[
False, False, False, False, True, True, True, True, True
])
self.joints = [0] * len(self.chain1.links)
self.joints[-4] = 0
self.target = [0.1, -0.2, 0.1]
self.frame_target = np.eye(4)
self.frame_target[:3, 3] = self.target
def test_urdf_chain(resources_path, interactive):
"""Test that we can open chain from a URDF file"""
chain1 = chain.Chain.from_urdf_file(os.path.join(resources_path,
"poppy_torso.URDF"),
base_elements=[
"base", "abs_z", "spine", "bust_y",
"bust_motors", "bust_x", "chest",
"r_shoulder_y"
],
last_link_vector=[0, 0.18, 0],
active_links_mask=[
False, False, False, False, True,
True, True, True, True
])
joints = [0] * len(chain1.links)
joints[-4] = 0
ax = plot_utils.init_3d_figure()
chain1.plot(joints, ax)
plt.savefig("out/chain1.png")
if interactive:
plt.show()
#Get physical chain:
my_chain = ikpy.chain.Chain.from_urdf_file("./resources/BA006N.urdf")
#CONFIGS:
NUM_OF_JOINTS = 7
NUM_OF_RUNS = int(1e7)
step = 30
to_z_mesh = range(0, 180+step, step)
to_x_mesh = range(-180, 180, step)
TO_Z_MESH, TO_X_MESH = np.meshgrid(to_z_mesh, to_x_mesh)
WANTED_PAIR_deg = np.array([TO_X_MESH.reshape(-1), TO_Z_MESH.reshape(-1)]).T
WANTED_PAIR = np.array([TO_X_MESH.reshape(-1), TO_Z_MESH.reshape(-1)]).T*np.pi/180
PLOT = True
PLOT = False
plt.ion()
ax = plot_utils.init_3d_figure()
#Check first:
self_check()
#prepare:
real_frames = np.empty(shape=(NUM_OF_RUNS, NUM_OF_JOINTS,4,4))
six_axis = np.random.random((NUM_OF_RUNS, NUM_OF_JOINTS))*720-360
pairs = np.empty(shape=(NUM_OF_RUNS, 2))
densities = np.empty(shape=(NUM_OF_RUNS, 1))
#and run:
pairs, densities = random_pose_sample(six_axis, pairs, densities)
#For output pose
nearest_to_anchor = []
for i in WANTED_PAIR_deg:
nearest_to_anchor.append(np.abs((pairs-i)).sum(axis=1).argmin())
Moves the robot to standard idle position, make it compliant, then close the connections.
"""
print('Shutting down the robot...')
self.init_angles()
self.robot.compliant = True
self.robot.close()
print('Robotic arm shut down. Power source can now be safely unplugged.')
if __name__ == '__main__': # A basic demonstration, presenting various features available with this class
# Full robot
arm = GripperArm('full')
ax = plot_utils.init_3d_figure()
# Reach first target, then close gripper
target = [0.215, -0.13, 0.3]
(fa, ma) = arm.goto_target(target, 1.5, wait = True)
th_epos = arm.chain.forward_kinematics(fa)[:3, 3]
current_epos = arm.get_endpoint_position()
print('\nTheoretical error to target [m]: {}\nActual error to target [m]: {}'.format(norm(target - th_epos), norm(target - current_epos) ) )
arm.chain.plot(fa, ax, target = target)
arm.set_clamp_position(25, 1, wait = True)
# Reach second target, then open gripper
target = [0.22, -0.09, 0.24]
(fa, ma) = arm.goto_target(target, 1.5, wait = True)
th_epos = arm.chain.forward_kinematics(fa)[:3, 3]
current_epos = arm.get_endpoint_position()
