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")