Source code for pykin.robot

import sys, os
import numpy as np
pykin_path = os.path.abspath(os.path.dirname(__file__)+"../" )
sys.path.append(pykin_path)

from pykin.kinematics.kinematics import Kinematics
from pykin.kinematics.transform import Transform
from pykin.models.urdf_model import URDFModel

[docs]class Robot(URDFModel): """ Initializes a robot object, as defined by a single corresponding robot URDF Args: fname (str): path to the urdf file. """ def __init__( self, fname=None, offset=Transform(), ): if fname is None: fname = pykin_path + "/asset/urdf/baxter/baxter.urdf" self._offset = offset super(Robot, self).__init__(fname) # self.transformations = None self.collision_utils = None self.kin = None self._setup_kinematics() def __str__(self): return f"""ROBOT : {self.robot_name} {self.links} {self.joints}""" def __repr__(self): return 'pykin.robot.{}()'.format(type(self).__name__) @property def offset(self): return self._offset @offset.setter def offset(self, offset): self._offset = offset
[docs] def show_robot_info(self): """ Shows robot's info """ print("*" * 100) print(f"Robot Information: \n{self}") print(f"robot's dof : {self.dof}") print(f"active joint names: \n{self.get_actuated_joint_names()}") print("*" * 100)
[docs] def compute_pose_error(self, target=np.eye(4), result=np.eye(4)): """ Computes pose error Args: target (np.array): target pose result (np.array): result pose Returns: error (np.array) """ error = np.linalg.norm(np.dot(result, np.linalg.inv(target)) - np.mat(np.eye(4))) return error
def _setup_kinematics(self): """ Sets instance of Kinematics """ self.kin = Kinematics(robot_name=self.robot_name, offset=self.offset, active_joint_names=self.get_actuated_joint_names(), base_name="", eef_name=None, frames=self.root ) self._init_transform() def _init_transform(self): """ Initializes robot's transformation """ thetas = np.zeros(self.dof) self.kin.forward_kinematics(thetas)
[docs] def set_desired_frame(self, base_name="", eef_name=None): """ Sets robot's desired frame Args: base_name (str): reference link name eef_name (str): end effector name """ self.kin.base_name = base_name self.kin.eef_name = eef_name if base_name == "": desired_base_frame = self.root else: desired_base_frame = self.find_frame(base_name + "_frame") self.desired_frames = self.generate_desired_frame_recursive(desired_base_frame, eef_name) self.kin.frames = self.desired_frames self.kin.active_joint_names = self.get_actuated_joint_names(self.kin.frames)
[docs] def reset_desired_frames(self): """ Resets robot's desired frame """ self.kin.frames = self.root self.kin.active_joint_names = self.get_actuated_joint_names()
@property def transformations(self): return self.kin._transformations @transformations.setter def transformations(self, transformations): self.transformations = transformations @property def active_joint_names(self): return self.kin._active_joint_names