def pyik_start_server(urdf, port, debug, root_link, end_link):
"""Start IK solver for a robot arm described by URDF
URDF Description of the selected arm.
"""
global arm
with urdf.open() as urdf_file:
arm = kp.build_serial_chain_from_urdf(
urdf_file.read(),
root_link_name="base_link",
end_link_name="ee_link",
)
app.run(host="127.0.0.1", port=port, debug=debug)
def test_jacobian3(self):
chain = kp.build_serial_chain_from_urdf(
open("examples/kuka_iiwa/model.urdf").read(), "lbr_iiwa_link_7")
th = [0.0, -np.pi / 4.0, 0.0, np.pi / 2.0, 0.0, np.pi / 4.0, 0.0]
jc = chain.jacobian(th)
np.testing.assert_almost_equal(
np.array([
[0, 1.41421356e-02, 0, 2.82842712e-01, 0, 0, 0],
[-6.60827561e-01, 0, -4.57275649e-01, 0, 5.72756493e-02, 0, 0],
[0, 6.60827561e-01, 0, -3.63842712e-01, 0, 8.10000000e-02, 0],
[0, 0, -7.07106781e-01, 0, -7.07106781e-01, 0, -1],
[0, 1, 0, -1, 0, 1, 0],
[1, 0, 7.07106781e-01, 0, -7.07106781e-01, 0, 0]
]), jc)
def test_jacobian1(self):
data = '<robot name="test_robot">'\
'<link name="link1" />'\
'<link name="link2" />'\
'<link name="link3" />'\
'<joint name="joint1" type="revolute">'\
'<origin xyz="0.0 0.0 0.0"/>'\
'<parent link="link1"/>'\
'<child link="link2"/>'\
'</joint>'\
'<joint name="joint2" type="revolute">'\
'<origin xyz="1.0 0.0 0.0"/>'\
'<parent link="link2"/>'\
'<child link="link3"/>'\
'</joint>'\
'</robot>'
chain = kp.build_serial_chain_from_urdf(data, 'link3')
jc = chain.jacobian([0.0, 0.0])
np.testing.assert_equal(
np.array([[0.0, 0.0], [1.0, 0.0], [0.0, 0.0], [0.0, 0.0],
[0.0, 0.0], [1.0, 1.0]]), jc)
def test_fkik(self):
data = '<robot name="test_robot">'\
'<link name="link1" />'\
'<link name="link2" />'\
'<link name="link3" />'\
'<joint name="joint1" type="revolute">'\
'<origin xyz="1.0 0.0 0.0"/>'\
'<parent link="link1"/>'\
'<child link="link2"/>'\
'</joint>'\
'<joint name="joint2" type="revolute">'\
'<origin xyz="1.0 0.0 0.0"/>'\
'<parent link="link2"/>'\
'<child link="link3"/>'\
'</joint>'\
'</robot>'
chain = kp.build_serial_chain_from_urdf(data, 'link3')
th1 = np.random.rand(2)
tg = chain.forward_kinematics(th1)
th2 = chain.inverse_kinematics(tg)
self.assertTrue(np.allclose(th1, th2, atol=1.0e-6))
import numpy as np
import kinpy as kp
chain = kp.build_serial_chain_from_urdf(
open("kuka_iiwa/model.urdf").read(), "lbr_iiwa_link_7")
print(chain)
print(chain.get_joint_parameter_names())
th = [0.0, -np.pi / 4.0, 0.0, np.pi / 2.0, 0.0, np.pi / 4.0, 0.0]
ret = chain.forward_kinematics(th, end_only=False)
print(ret)
viz = kp.Visualizer()
viz.add_robot(ret, chain.visuals_map(), mesh_file_path='kuka_iiwa/', axes=True)
viz.spin()
import numpy as np
import kinpy as kp
arm = kp.build_serial_chain_from_urdf(
open("ur/ur.urdf").read(),
root_link_name="base_link",
end_link_name="ee_link",
)
fk_solution = arm.forward_kinematics(
np.zeros(len(arm.get_joint_parameter_names())))
print(fk_solution)
"""Test that displays an arm configuration using kinpy.
Written by Simon Kapen '24 in Fall 2021.
"""
import math
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import art3d
from arm_node import Node
import kinpy as kp
chain = kp.build_chain_from_urdf(
open("models/SimpleArmModelforURDF.URDF").read())
chain = kp.build_serial_chain_from_urdf(
open("models/SimpleArmModelforURDF.URDF").read(), "hand_1", "base_link")
angle_config = kp.ik.inverse_kinematics(chain,
kp.Transform(pos=[0.2, 0.2, 0.2]))
ret = chain.forward_kinematics(angle_config, end_only=False)
print(ret)
node = Node.from_point([.2, .2, .2])
print(node.joint_positions)
# # Generate locations of each joint. Input angles here.
# th = {'Rev2': 0, 'Rev3': 0, 'Rev4': 0, 'Rev5': 0, 'Rev6': math.pi/2}
# ret = chain.forward_kinematics(th)
#
# print(chain.get_joint_parameter_names())
# print(chain)
# print(ret)