-
Notifications
You must be signed in to change notification settings - Fork 50
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
425c7fd
commit 213bc8d
Showing
7 changed files
with
128 additions
and
25 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,4 +1,5 @@ | ||
from .utils import check_urdf | ||
from .fk import fk | ||
from .ik import ik | ||
from .fk import get_fk_from_model | ||
from .jacobian import get_jacobian_from_model | ||
from .ik import get_ik_from_model | ||
from .ik_dual import ik_dual |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,31 @@ | ||
import numpy as np | ||
|
||
|
||
def get_jacobian_from_model(file_path, end_link_name, joint_values): | ||
""" | ||
Get the Jacobian matrix J of the robot from the 3D model, \dot x= J \dot q | ||
:param file_path: | ||
:param joint_values: | ||
:return: | ||
""" | ||
import kinpy as kp | ||
if file_path.endswith('.urdf'): | ||
chain = kp.build_serial_chain_from_urdf(open(file_path).read(), end_link_name=end_link_name) | ||
elif file_path.endswith('.xml'): | ||
chain = kp.build_serial_chain_from_mjcf(open(file_path).read(), end_link_name=end_link_name) | ||
|
||
J = chain.jacobian(joint_values) | ||
|
||
ret = chain.forward_kinematics(joint_values, end_only=False) | ||
|
||
# viz = kp.Visualizer() | ||
# viz.add_robot(ret, chain.visuals_map(), mesh_file_path="/home/ubuntu/Github/Manipulation/kinpy/examples/kuka_iiwa/", axes=True) | ||
# viz.spin() | ||
|
||
return J | ||
|
||
|
||
if __name__ == '__main__': | ||
urdf_path = "/home/ubuntu/Github/Manipulation/kinpy/examples/kuka_iiwa/model.urdf" | ||
joint_values = [0.0, -np.pi / 4.0, 0.0, np.pi / 2.0, 0.0, np.pi / 4.0, -np.pi / 4.0] | ||
get_jacobian_from_model(urdf_path, end_link_name='lbr_iiwa_link_7', joint_values=joint_values) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1 @@ | ||
from .mpb import get_mpb_from_urdf |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,37 @@ | ||
import numpy as np | ||
|
||
from rofunc.utils.robolab.kinematics.jacobian import get_jacobian_from_model | ||
from rofunc.utils.visualab.ellipsoid import ellipsoid_plot3d | ||
|
||
|
||
def get_mpb_from_model(file_path, end_link_name, joint_values, show=False): | ||
""" | ||
Get the manipulability of the robot from the 3D model | ||
:param file_path: path to the robot model file | ||
:return: the manipulability of the robot | ||
""" | ||
J = get_jacobian_from_model(file_path, end_link_name, joint_values) | ||
|
||
# calculate the velocity manipulability | ||
A = J @ J.T | ||
vel_eig_values, vel_eig_vectors = np.linalg.eig(A) | ||
vel_M = np.sqrt(np.linalg.det(A)) | ||
|
||
# calculate the force manipulability | ||
force_eig_values = np.reciprocal(vel_eig_values) | ||
force_eig_vectors = vel_eig_vectors | ||
A_inv = np.linalg.inv(A) | ||
# force_eig_values, force_eig_vectors = np.linalg.eig(A_inv) | ||
force_M = np.sqrt(np.linalg.det(A_inv)) | ||
|
||
if show: | ||
ellipsoid_plot3d(np.array([vel_eig_values[:3], force_eig_values[:3]]), mode='given', | ||
Rs=np.array([vel_eig_vectors[:3, :3], vel_eig_vectors[:3, :3]])) | ||
|
||
return vel_M, vel_eig_values, vel_eig_vectors, force_M, force_eig_values, force_eig_vectors | ||
|
||
|
||
if __name__ == '__main__': | ||
urdf_path = "/home/ubuntu/Github/Manipulation/kinpy/examples/kuka_iiwa/model.urdf" | ||
joint_values = [0, -np.pi / 4.0, -np.pi / 4.0, np.pi / 2.0, 0.0, np.pi / 4.0, -np.pi / 4.0] | ||
get_mpb_from_model(urdf_path, end_link_name='lbr_iiwa_link_7', joint_values=joint_values, show=True) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters