def __init__(self,
urdf=(dirname(abspath(__file__)) + "/urdf/kinematics.urdf")):
"""
Allows to visualize and manipulate joint angles of the given urdf
:param urdf: path of the urdf file
:type urdf: str
"""
self.robot = robot.robot(urdf, [])
"torso:11", "r_shoulder_z", "right_shoulder:11", "r_shoulder_y",
"right_collarbone:11", "r_arm_x", "right_upper_arm:11", "r_elbow_y",
"right_lower_arm:11", "r_wrist_z", "right_wrist:11", "r_wrist_x",
"right_palm:11", "r_ringfingers_x"
],
# Not all the joints listed have to take part in the movement
# This is a mask blending out joints inbetween the chain
# (here we do not want to involve the finger joints)
active_joints_mask=[
False, True, True, True, True, True, True, False, False, False
])
# For visualization we built he a full robot model
# again using the URDF
nico_right_chain = chain_definitions.nico_right_chain_active
nico = robot.robot(nico_path, nico_right_chain)
# We use some poses, where we can be sure, that they are mechanically
# reachable (cause we have used forward kinematics to generate the dataset, see this in the examples)
with open(gaikpy.get_nico_data_path() + '/nico_right_20_new.p', 'rb') as f:
sample_set = pickle.load(f)
# Go through all the samples in file
# The contains pairs of poses in Eukledian and in the joint space
# Of cours we use only the Eukledian ones
for idx, sample in enumerate(sample_set):
# joi is the data in joint space for the robot (not used)
# sfwr is the pose in Euklidan space
(joi, sfwr) = sample
print("Original pose: \n" + str(sfwr))
nicorightchain = chain.Chain.from_urdf_file(
nico_path,
base_elements=[
"torso:11", "r_shoulder_z", "right_shoulder:11", "r_shoulder_y",
"right_collarbone:11", "r_arm_x", "right_upper_arm:11", "r_elbow_y",
"right_lower_arm:11", "r_wrist_z", "right_wrist:11", "r_wrist_x",
"right_palm:11", "r_ringfingers_x"
],
active_joints_mask=[
False, True, True, True, True, True, True, False, False, False
])
if visualisation:
nico = robot.robot(nico_path,
nico_right_chain,
off_screen=False,
ego_view=True,
visualisation=True)
class TestNICORobot(unittest.TestCase):
def setUp(self):
self.rightchain = chain.Chain.from_urdf_file(
nico_path,
base_elements=[
"torso:11", "r_shoulder_z", "right_shoulder:11",
"r_shoulder_y", "right_collarbone:11", "r_arm_x",
"right_upper_arm:11", "r_elbow_y", "right_lower_arm:11",
"r_wrist_z", "right_wrist:11", "r_wrist_x", "right_palm:11",
"r_ringfingers_x"
],