import numpy as np
import time
from pykin.kinematics.transform import Transform
JOINT_TYPE_MAP = {'revolute' : 'revolute',
'fixed' : 'fixed',
'prismatic' : 'prismatic'}
LINK_TYPE_MAP = {'cylinder' : 'cylinder',
'sphere' : 'sphere',
'box' : 'box',
'mesh' : 'mesh'}
LINK_TYPES = ['box', 'cylinder', 'sphere', 'capsule', 'mesh']
[docs]class ShellColors:
HEADER = '\033[95m'
OKBLUE = '\033[94m'
OKCYAN = '\033[96m'
OKGREEN = '\033[92m'
WARNING = '\033[93m'
FAIL = '\033[91m'
ENDC = '\033[0m'
BOLD = '\033[1m'
UNDERLINE = '\033[4m'
[docs]class Baxter:
left_e0_fixed_offset = Transform(rot=[0.5, 0.5, 0.5, 0.5], pos=[0.107, 0., 0. ])
left_w0_fixed_offset = Transform(rot=[0.5, 0.5, 0.5, 0.5], pos=[0.088, 0., 0. ])
right_e0_fixed_offset = Transform(rot=[0.5, 0.5, 0.5, 0.5], pos=[0.107, 0., 0. ])
right_w0_fixed_offset = Transform(rot=[0.5, 0.5, 0.5, 0.5], pos=[0.088, 0., 0. ])
[docs] @staticmethod
def add_visual_link(link_transforms, f):
if "left_lower_shoulder" in f.link.name:
link_transforms["left_upper_elbow_visual"] = np.dot(link_transforms["left_lower_shoulder"],
Baxter.left_e0_fixed_offset)
if "left_lower_elbow" in f.link.name:
link_transforms["left_upper_forearm_visual"] = np.dot(link_transforms["left_lower_elbow"],
Baxter.left_w0_fixed_offset)
if "right_lower_shoulder" in f.link.name:
link_transforms["right_upper_elbow_visual"] = np.dot(link_transforms["right_lower_shoulder"],
Baxter.right_e0_fixed_offset)
if "right_lower_elbow" in f.link.name:
link_transforms["right_upper_forearm_visual"] = np.dot(link_transforms["right_lower_elbow"],
Baxter.right_w0_fixed_offset)
[docs]def convert_thetas_to_dict(active_joint_names, thetas):
"""
Check if any pair of objects in the manager collide with one another.
Args:
active_joint_names (list): actuated joint names
thetas (sequence of float): If not dict, convert to dict ex. {joint names : thetas}
Returns:
thetas (dict): Dictionary of actuated joint angles
"""
if not isinstance(thetas, dict):
assert len(active_joint_names) == len(thetas
), f"""the number of robot joint's angle is {len(active_joint_names)},
but the number of input joint's angle is {len(thetas)}"""
thetas = dict((j, thetas[i]) for i, j in enumerate(active_joint_names))
return thetas
[docs]def logging_time(original_fn):
"""
Decorator to check time of function
"""
def wrapper_fn(*args, **kwargs):
start_time = time.time()
result = original_fn(*args, **kwargs)
end_time = time.time()
print(f"WorkingTime[{original_fn.__name__}]: {end_time-start_time:.4f} sec\n")
return result
return wrapper_fn
[docs]def convert_string_to_narray(str_input):
"""
Args:
str_input (str): string
Returns:
np.array: Returns string to np.array
"""
if str_input is not None:
return np.array([float(data) for data in str_input.split()])
[docs]def calc_pose_error(tar_pose, cur_pose, EPS):
"""
Args:
tar_pos (np.array): target pose
cur_pos (np.array): current pose
EPS (float): epsilon
Returns:
np.array: Returns pose error
"""
def rot_to_omega(R):
# referred p36
el = np.array(
[[R[2, 1] - R[1, 2]],
[R[0, 2] - R[2, 0]],
[R[1, 0] - R[0, 1]]]
)
norm_el = np.linalg.norm(el)
if norm_el > EPS:
w = np.dot(np.arctan2(norm_el, np.trace(R) - 1) / norm_el, el)
elif (R[0, 0] > 0 and R[1, 1] > 0 and R[2, 2] > 0):
w = np.zeros((3, 1))
else:
w = np.dot(np.pi/2, np.array([[R[0, 0] + 1], [R[1, 1] + 1], [R[2, 2] + 1]]))
return w
pos_err = np.array([tar_pose[:3, -1] - cur_pose[:3, -1]])
rot_err = np.dot(cur_pose[:3, :3].T, tar_pose[:3, :3])
w_err = np.dot(cur_pose[:3, :3], rot_to_omega(rot_err))
return np.vstack((pos_err.T, w_err))
[docs]def limit_joints(joint_angles, lower, upper):
"""
Set joint angle limit
Args:
joint_angles (sequence of float): joint angles
lower (sequence of float): lower limit
upper (sequence of float): upper limit
Returns:
joint_angles (sequence of float): Returns limited joint angle
"""
if lower is not None and upper is not None:
for i in range(len(joint_angles)):
if joint_angles[i] < lower[i]:
joint_angles[i] = lower[i]
if joint_angles[i] > upper[i]:
joint_angles[i] = upper[i]
return joint_angles
[docs]def get_robot_collision_geom(link):
"""
Get robot geometry from link
Args:
link (Link): robot's link
Returns:
name (str): geom's name
gtype: geom's type
gparam: geom's param
"""
def get_cylinder_param(link):
radius = float(link.collision.gparam.get('radius'))
length = float(link.collision.gparam.get('length'))
return (radius, length)
def get_spehre_param(link):
radius = float(link.collision.gparam.get('radius'))
return radius
def get_box_param(link):
size = list(link.collision.gparam.get('size'))
return size
name = None
gtype = None
gparam = None
if link.collision.gtype == "cylinder":
name = link.name
gtype = link.collision.gtype
gparam = get_cylinder_param(link)
elif link.collision.gtype == "sphere":
name = link.name
gtype = link.collision.gtype
gparam = get_spehre_param(link)
elif link.collision.gtype == "box":
name = link.name
gtype = link.collision.gtype
gparam = get_box_param(link)
return name, gtype, gparam