def __init__(self): self.chain = Chain( name="pupper_ergo", links=[ OriginLink(), URDFLink( name="twistybase", translation_vector=[0, 0, 0.03], orientation=[0, 0, 0], rotation=[0, 0, 1], ), URDFLink( name="shoulder", translation_vector=[0, 0, 0.0285], orientation=[0, 0, 0], rotation=[1, 0, 0], ), URDFLink( name="elbow", translation_vector=[0, 0, 0.085], orientation=[0, 0, 0], rotation=[1, 0, 0], ), URDFLink( name="tip", translation_vector=[0, 0.155, 0], orientation=[1.57, 0, 0], rotation=[1, 0, 0], ), ], )
def backhoe(): backhoe_def = BackhoeDefinition() # Actuators # Origin is at bottom link of boom piston chain = Chain(name='backhoe', links=[ OriginLink(), URDFLink( name="boom", bounds=(0, 90), translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="stick", translation_vector=[25, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="bucket", translation_vector=[22, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ) ]) return chain pass
def __init__(self, direction): self.state = [0, 0, 0, 0, 0] if direction == "left": sign = -1 else: sign = 1 self.chain = Chain( name=direction + "_arm", links=[ OriginLink(), URDFLink( name="shoulder", translation_vector=[8 * sign, 0, 0], orientation=[0, 0, math.radians(30 * sign)], rotation=[0, 0, 1], ), URDFLink(name="backarm", translation_vector=[16 * sign, 0, 0], orientation=[0, 0, math.radians(90 * sign)], rotation=[0, 0, 1]), URDFLink(name="forearm", translation_vector=[16 * sign, 0, 0], orientation=[0, 0, math.radians(30 * sign)], rotation=[0, 0, 1]), URDFLink(name="hand", translation_vector=[9 * sign, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1]), ])
def __init__(self): # Second robo, with dynamixel self.armLength = 4 self.smallArmLength = 4 lim = math.pi / 4 # lim = 7*math.pi/9 self.chain = Chain( name='arm', links=[ OriginLink(), URDFLink(name="base", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-math.pi, math.pi)), URDFLink(name="first", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], bounds=(-lim, lim)), URDFLink(name="second", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, -1, 0], bounds=(-lim, lim)), URDFLink(name="third", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, 1, 0], bounds=(-lim, lim)), URDFLink(name="fourth", translation_vector=[0, 0, self.armLength], orientation=[0, 0, 0], rotation=[0, -1, 0], bounds=(-lim, lim)), URDFLink( name="tip", translation_vector=[0, 0, self.smallArmLength], orientation=[0, 0, 0], rotation=[0, 0, 0], ) ]) self.xMin = -3 * self.armLength - self.smallArmLength self.xMax = 3 * self.armLength + self.smallArmLength self.yMin = -3 * self.armLength - self.smallArmLength self.yMax = 3 * self.armLength + self.smallArmLength self.zMin = 0 self.zMax = 3 * self.armLength + self.smallArmLength self.radToDegreeFactor = 180 / math.pi self.degreeToRadFactor = math.pi / 180 self.numberOfLinks = len(self.chain.links)
def __init__(self, active_joints=[1, 1, 1, 1], base_translation=[0, 0, 0.84], # x, y, z base_orientation=[0, 0, np.pi/2], # x, y, z tool_translation=[0, 0, 0], tool_orientation=[0, 0, 0]): links=[OriginLink(), URDFLink(name="wam/links/base", translation_vector=base_translation, # translation of frame orientation=base_orientation, # orientation of frame rotation=[0, 0, 0]), # joint axis [0, 0, 0] -> no joint URDFLink(name="wam/links/shoulder_yaw", translation_vector=[0, 0, 0.16], orientation=[0, 0, 0], rotation=[0, 0, 1]), URDFLink(name="wam/links/shoulder_pitch", translation_vector=[0, 0, 0.186], orientation=[0, 0, 0], rotation=[1, 0, 0]), URDFLink(name="wam/links/shoulder_roll", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1]), URDFLink(name="wam/links/upper_arm", translation_vector=[0, -0.045, 0.550], orientation=[0, 0, 0], rotation=[1, 0, 0]), URDFLink(name="wam/links/tool_base_wo_plate", translation_vector=[0, 0.045, 0.350], orientation=[0, 0, 0], rotation=[0, 0, 0]), URDFLink(name="wam/links/tool_base_w_plate", translation_vector=[0, 0, 0.008], orientation=[0, 0, 0], rotation=[0, 0, 0]), URDFLink(name="wam/links/tool", translation_vector=tool_translation, orientation=tool_orientation, rotation=[0, 0, 0]) ] self.all_joints = [False, False, True, True, True, True, False, False, False] self.active_joints = list(map(lambda x:x==1, active_joints)) self.active_links = [False, False, *self.active_joints, False, False, False] Chain.__init__(self, name='wam4', active_links_mask=self.active_links, links=links)
def _get_right_arm(self) -> Chain: return Chain(name="right_arm", links=[ OriginLink(), URDFLink(name="shoulder_pitch", translation_vector=[0.235, 0.565, 0.075], orientation=[-np.pi / 2, 0, 0], rotation=[-1, 0, 0], bounds=(-1.0472, 3.12414)), URDFLink(name="shoulder_yaw", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], bounds=(-1.5708, 1.5708)), URDFLink(name="shoulder_roll", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-0.785398, 0.785398)), URDFLink(name="elbow_pitch", translation_vector=[0, 0, -0.235], orientation=[0, 0, 0], rotation=[-1, 0, 0], bounds=(0, 2.79253)), URDFLink(name="wrist_roll", translation_vector=[0, 0, -0.15], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-1.5708, 1.5708)), URDFLink(name="wrist_pitch", translation_vector=[0, 0, 0], orientation=[0, 0, 0], rotation=[-1, 0, 0], bounds=(0, 1.5708)), URDFLink(name="mitten", translation_vector=[0, 0, -0.0625], orientation=[0, 0, 0], rotation=[0, 0, 0])])
def example_chain(): left_arm_chain = Chain(name='left_arm', links=[ OriginLink(), URDFLink( name="shoulder", translation_vector=[-10, 0, 5], orientation=[0, 1.57, 0], rotation=[0, 1, 0], ), URDFLink( name="elbow", translation_vector=[25, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="wrist", translation_vector=[22, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ) ]) return left_arm_chain
def __init__(self, *args, **kwargs): self.chain = Chain( name='left_arm', links=[ OriginLink(), URDFLink( name="shoulder_left", translation_vector=[0.13182, -2.77556e-17, 0.303536], orientation=[-0.752186, -0.309384, -0.752186], rotation=[0.707107, 0, 0.707107], ), URDFLink( name="proximal_left", translation_vector=[-0.0141421, 0, 0.0424264], orientation=[-7.77156e-16, 7.77156e-16, 5.55112e-17], rotation=[-0.707107, 0, 0.707107], ), URDFLink( name="distal_left", translation_vector=[0.193394, -0.0097, 0.166524], orientation=[1.66533e-16, -7.21645e-16, 3.88578e-16], rotation=[0, -1, 0], ) ])
from ikpy.chain import Chain from ikpy.link import OriginLink, URDFLink import ikpy.utils.plot as plot_utils import matplotlib.pyplot as plt import numpy as np left_arm_chain = Chain(name='left_arm', links=[ OriginLink(), URDFLink( name="shoulder", translation_vector=[-10, 0, 5], orientation=[0, 1.57, 0], rotation=[0, 1, 0], ), URDFLink( name="elbow", translation_vector=[25, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ), URDFLink( name="wrist", translation_vector=[22, 0, 0], orientation=[0, 0, 0], rotation=[0, 1, 0], ) ]) fig, ax = plot_utils.init_3d_figure() left_arm_chain.plot([0, 0, 0, 0], ax)
def __init__(self): self._done = False self._head = baxter_interface.Head() self._left_arm = baxter_interface.limb.Limb('left') self._right_arm = baxter_interface.limb.Limb('right') print(self._left_arm.joint_names()) print(self._left_arm.joint_angles()) print(self._left_arm.joint_velocities()) print(self._left_arm.joint_efforts()) print(self._left_arm.endpoint_pose()) print(self._left_arm.endpoint_velocity()) print(self._left_arm.endpoint_effort()) self._left_arm_chain = Chain( name='left_arm', active_links_mask=[ False, False, True, True, True, True, True, True, True, False ], links=[ OriginLink(), URDFLink( name="left_torso_arm_mount", translation_vector=[0.024645, 0.219645, 0.118588], orientation=[0, 0, 0.7854], rotation=[0, 0, 1], ), URDFLink(name="left_s0", translation_vector=[0.055695, 0, 0.011038], orientation=[0, 0, 0], rotation=[0, 0, 1], bounds=(-1.70167993878, 1.70167993878)), URDFLink(name="left_s1", translation_vector=[0.069, 0, 0.27035], orientation=[-1.57079632679, 0, 0], rotation=[0, 0, 1], bounds=(-2.147, 1.047)), URDFLink(name="left_e0", translation_vector=[0.102, 0, 0], orientation=[1.57079632679, 0, 1.57079632679], rotation=[0, 0, 1], bounds=(-3.05417993878, 3.05417993878)), URDFLink(name="left_e1", translation_vector=[0.069, 0, 0.26242], orientation=[-1.57079632679, -1.57079632679, 0], rotation=[0, 0, 1], bounds=(-0.05, 2.618)), URDFLink(name="left_w0", translation_vector=[0.10359, 0, 0], orientation=[1.57079632679, 0, 1.57079632679], rotation=[0, 0, 1], bounds=(-3.059, 3.059)), URDFLink(name="left_w1", translation_vector=[0.01, 0, 0.2707], orientation=[-1.57079632679, -1.57079632679, 0], rotation=[0, 0, 1], bounds=(-1.57079632679, 2.094)), URDFLink(name="left_w2", translation_vector=[0.115975, 0, 0], orientation=[1.57079632679, 0, 1.57079632679], rotation=[0, 0, 1], bounds=(-3.059, 3.059)), URDFLink( name="left_hand", translation_vector=[0, 0, 0.11355], orientation=[0, 0, 0], rotation=[0, 0, 1], ) ]) #self._left_arm_chain = ikpy.chain.Chain.from_urdf_file("/home/jbunker/ros_ws/src/baxter_common/baxter_description/urdf/baxter.urdf") # verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() print("Running. Ctrl-c to quit")