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):
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 solve_shoulder_angles(v_o, v_new, a1, a2, b1, b2):
chain = Chain(
name='arm',
links=[
#OriginLink(),
URDFLink(
name="shoulder1",
translation_vector=[0, 0, 0],
orientation=[0, 0, 0],
bounds=b1,
rotation=a1,
),
URDFLink(
name="shoulder2",
translation_vector=[0, 0, 0],
orientation=[0, 0, 0],
bounds=b2,
rotation=a2,
),
URDFLink(
name="elbow",
translation_vector=v_o,
orientation=[0, 0, 0],
rotation=[0, 0, 0],
)
])
target_frame = np.eye(4)
target_frame[:3, 3] = v_new
angles = chain.inverse_kinematics(target_frame)[:2]
return np.rad2deg(angles)
class ArmIK:
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],
bounds=(-90, 90),
),
URDFLink(
name="shoulder",
translation_vector=[0, 0, 0.0285],
orientation=[0, 0, 0],
rotation=[1, 0, 0],
bounds=(-90, 90),
),
URDFLink(
name="elbow",
translation_vector=[0, 0, 0.085],
orientation=[0, 0, 0],
rotation=[1, 0, 0],
bounds=(-90, 90),
),
URDFLink(
name="tip",
translation_vector=[0, 0.155, 0],
orientation=[1.57, 0, 0],
rotation=[1, 0, 0],
bounds=(-90, 90),
),
],
)
@staticmethod
def _ik2pupperarm(joints):
return np.rad2deg([joints[1], -joints[2], -joints[3], 0.0])
@staticmethod
def _fk2pupperarm(joints):
return np.deg2rad([joints[1], -joints[2], -joints[3], 0.0, 0.0])
@staticmethod
def _makeMoveCmd(degs):
print(f"move([{','.join([str(x) for x in np.around(degs, 2)])}])")
def pos2joints(self, target):
joints = self.chain.inverse_kinematics(target)
return self._ik2pupperarm(joints)
def joints2pos(self, joints):
joints = self._fk2pupperarm(joints)
return self.chain.forward_kinematics(joints)[:3, 3]
def __init__(self):
self.my_chain = Chain.from_urdf_file("./panda_with_bound.URDF")
self.my_chain_ = Chain.from_urdf_file(
"./panda_with_bound_nofinger_center.URDF")
self.state = [0] * 10
self.motorPosList = [0] * 7
# self.target_position = [0]*3
TargetList = [[0.5, 0, 0], [0.4, 0, 0], [0.6, 0, 0], [0.4, 0.2, 0],
[0.5, 0.2, 0], [0.6, 0.2, 0], [0.4, -0.2, 0],
[0.5, -0.2, 0], [0.6, -0.2, 0]]
self.target_position = np.array(TargetList[random.randint(0, 8)])
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)
class Arm:
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 get_positions(self):
rv = []
positions = self.chain.forward_kinematics(self.state,
full_kinematics=True)
for position in positions[1:]:
xyEtc = position[:, 3]
x = xyEtc[0]
y = xyEtc[1]
rv.append((x, y))
return rv
def go_directly_to_position(self, x, y, book):
rad = math.radians(book.openness / 2)
if (x < book.center_x_cm):
rad *= -1
joint_positions = self.chain.inverse_kinematics(
target_position=[x, y, 0], target_orientation=[0, 0, 0.2])
self.state = joint_positions
class KSOLVER:
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],
)
])
def solve(self, wrist_postion):
"""Performs inverse kinematics and returns joint angles in rads
For now uses one target vector at the wrist position for inverse kinematics.
For the future, should use additional target vectors and match the elbow
"""
# TODO: Fix coordinate system transform for extra degree of freedom rotation
# Setup target vector with target position at wrist
target_vector = np.array(
[wrist_postion[0], wrist_postion[1], wrist_postion[2]])
target_frame = np.eye(4)
target_frame[:3, 3] = target_vector
joint_angles = self.chain.inverse_kinematics(target_frame)[1:3]
print('Joint angles:', joint_angles)
print('Computed position vector:', self.chain.forward_kinematics(self.chain.inverse_kinematics(target_frame))[:3, 3], \
'Original position vector:', target_frame[:3, 3])
joints = self.chain.inverse_kinematics(target_frame)[1:3]
return np.array([0, joints[1]])
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, 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 __init__(self):
self.urdf = Chain.from_urdf_file("URDF/dofbot.urdf")
self.dofbot = Arm_Lib.Arm_Device()
self.target = []
self.joints = []
self.is_closed = False
self.gripper = OPEN_GRIPPER
self.standby_pose = [90,180,0,0,90,self.gripper]
def __init__(self):
self.sup = Supervisor()
self.timeStep = int(4 * self.sup.getBasicTimeStep())
# Create the arm chain from the URDF
self.armChain = Chain.from_urdf_file('ur5e.urdf')
self.arm_motors = self.getARMJoint()
self.grip_motors = self.getGripperJoint()
def _init_forward_kinematics(self, urdf_file):
gripper_tip_elements = get_chain_from_joints(
urdf_file,
joints=[
'right_arm_mount', 'right_j0', 'right_j1', 'right_j2',
'right_j3', 'right_j4', 'right_j5', 'right_j6', 'right_hand',
'right_gripper_base_joint', 'right_gripper_tip_joint'
])
self.fk_chain = Chain.from_urdf_file(
urdf_file,
base_elements=gripper_tip_elements,
active_links_mask=[True] + 8 * [True] + 3 * [False])
def get_armchain(self) -> Chain:
"""
with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file:
filename = file.name
file.write(self.getUrdf().encode('utf-8'))
"""
"""
with open("robot.urdf","w") as f:
f.write(self.getUrdf())
#"""
arm_chain = Chain.from_urdf_file("../robot_control/robot.urdf")
return arm_chain
def get_torso_left_arm():
return Chain.from_urdf_file("./poppy_torso.URDF",
base_elements=[
"base", "abs_z", "spine", "bust_y",
"bust_motors", "bust_x", "chest",
"l_shoulder_y"
],
last_link_vector=[0, 0.18, 0],
active_links_mask=[
False, False, False, False, True, True,
True, True, True
],
name="poppy_torso_left_arm")
def torso_right_arm():
chain1 = Chain.from_urdf_file("../resources/poppy_torso/poppy_torso.URDF",
base_elements=[
"base", "abs_z", "spine", "bust_y",
"bust_motors", "bust_x", "chest",
"r_shoulder_y"
],
last_link_vector=[0, 0.18, 0],
active_links_mask=[
False, False, False, False, True, True,
True, True, True
],
name="poppy_torso_right_arm")
return chain1
def __init__(self):
rospy.init_node('kinematics')
rospy.loginfo("Kinematic thingy initializin' here...")
self.chain = Chain.from_urdf_file("urdf/rbx1_urdf.urdf",
active_links_mask=[
False, True, True, True, True,
True, True, True, False, False
])
self.publisher = rospy.Publisher('/joint_states',
JointState,
queue_size=10)
self.seq = 0
print(self.chain)
def solve_hip_angles(v_o, v_new, a1, a2, a3, b1, b2, b3):
chain = Chain(
name='arm',
links=[
#OriginLink(),
URDFLink(
name="hip1",
translation_vector=[0, 0, 0],
orientation=[0, 0, 0],
bounds=b1,
rotation=a1,
),
URDFLink(
name="hip2",
translation_vector=[0, 0, 0],
orientation=[0, 0, 0],
bounds=b2,
rotation=a2,
),
URDFLink(
name="hip3",
translation_vector=[0, 0, 0],
orientation=[0, 0, 0],
bounds=b3,
rotation=a3,
),
URDFLink(
name="knee",
translation_vector=v_o,
orientation=[0, 0, 0],
rotation=[0, 0, 0],
)
])
target_frame = np.eye(4)
target_frame[:3, 3] = v_new
angles = chain.inverse_kinematics(target_frame)[:3]
return np.rad2deg(angles)
def __init__(self,
motorList,
psList=None,
urdf_file_path="panda_with_bound.urdf"):
"""
@param
motorList: 各關節的驅動器。
psList: position sensor list，各關節的角度偵測器
urdf_file_path: 手臂的URDF檔案位置
注意，以上參數的關節個數必須相同
"""
self.motorList = motorList
self.psList = psList
self.armChain = Chain.from_urdf_file(urdf_file_path)
self.script = None
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],
)
])
def __init__(self):
self.supervisor = Supervisor()
self.timeStep = int(4 * self.supervisor.getBasicTimeStep())
# Create the arm chain from the URDF
with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file:
filename = file.name
file.write(self.supervisor.getUrdf().encode('utf-8'))
armChain = Chain.from_urdf_file(filename)
armChain.active_links_mask[0] = False
# Initialize the arm motors and encoders.
self.motors = []
for link in armChain.links:
if 'motor' in link.name:
motor = self.supervisor.getDevice(link.name)
motor.setVelocity(1.0)
position_sensor = motor.getPositionSensor()
position_sensor.enable(self.timeStep)
self.motors.append(motor)
self.arm = self.supervisor.getSelf()
self.positions = [0 for _ in self.motors]
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
# ),
# URDFLink(
# name="claw_elbow",
# translation_vector=[8, 0, 0],
# orientation=[0, -1.57, 0],
# rotation=[0, 1, 0]
# ),
# URDFLink(
# name="claw_rotate",
# translation_vector=[5, 0, 0],
# orientation=[0, 0, 0],
# rotation=[1, 0, 0],
# ),
# ])
claw_chain = Chain.from_urdf_file("arm_chain.urdf")
ax = plt.figure().add_subplot(111, projection="3d")
# plot_chain = claw_chain.plot(claw_chain.inverse_kinematics([
# [1, 0, 0, 0],
# [0, 1, 0, 0],
# [0, 0, 1, 0],
# [0, 0, 0, 1]
# ]), ax)
print(
claw_chain.inverse_kinematics([[1, 0, 0, 2], [0, 1, 0, 2], [0, 0, 1, 1],
[0, 0, 0, 1]]))
# plt.show(plot_chain)
receiver = robot.getReceiver('receiver')
receiver.enable(32)
receiver.setChannel(1)
# initialize motors
mot = []
motNames = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
for name in motNames:
mot.append(robot.getMotor(name))
# print("Got " + name)
#print(robot)
my_chain = Chain.from_urdf_file("../../ur10.urdf")
#sprint(my_chain.links)
a = np.array([6.0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 7.0])
b = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
temo = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
z = 1
T1 = np.array([[0.0,0.0,0.0,1.5],\
[0.0,0.0,0.0,1.0],\
[0.0,0.0,0.0,2.0],\
[0.0,0.0,0.0,1.0]])
tempAngles = []
def get_chain(name_templates, side):
assert side in ['l', 'r']
element_names = ['body_link'] + list(
map(lambda s: s.format(side), name_templates))
return Chain.from_urdf_file(DARWIN_URDF, base_elements=element_names)
import numpy
import matplotlib.pyplot as plt
from util.error_handling import nostderr
import numpy as np
import math
import random
import ikpy as IKPY
from util import line
from typing import List
from ikpy.chain import Chain
# Global arm configuration - IMPORTANT: wraps with nostderr() to hide command line errors.
ik_py = True #boolean flag: True if ik_py, False if Kinpy
with nostderr():
arm_chain = Chain.from_urdf_file("models/XArm.urdf")
class Node(object):
"""
A node generated in a RRT graph.
Args:
configuration: list of joint angles in radians. Corresponds to the 6 degrees of freedom on the arm.
a0-- the pan angle of the base
a1-- the lift angle of the base
a2-- the lift angle of the elbow
a3-- the pan angle of the elbow
a4-- the pan angle of the wrist
(a5-- how big to open the end effector -- not programmed)
Attributes:
bounds: An array of float arrays listing the lower and upper bounds of each arm angle.
ep_out = dev[0][(0, 0)][1]
print('')
print('Exit by pressing any button on the SpaceNavigator')
print('')
Z_push = 0
old_Z_push = 0
R_list = [0, 0, 0]
old_R_list = 0
Button_number = 0
#ロボットアームの初期設定#####################################
my_chain = Chain.from_urdf_file("cybathlon_robotarm.URDF")
Now_pos = my_chain.forward_kinematics([0] * 4)[:, 3][0:3] #展開後の初期姿勢を設定
##############################################################
#B3M(ロボットアーム展開)######################################
my_chain_b3m = B3m_speed_servo_lib.B3M_class()
#my_chain_b3m.start_arm()
##############################################################
run = True
while run:
try:
data = dev.read(ep_in.bEndpointAddress, ep_in.bLength, 0)
#Z軸のプッシュ判定
if data[0] == 1:
old_Z_push = copy.deepcopy(Z_push)
from controller import Supervisor
from controller import Motor
from controller import Camera
from controller import DistanceSensor
from controller import TouchSensor
import endpoint_env
import preprocessing
from pipe import make_pipe, close_pipe, open_write_pipe, open_read_pipe, write_to_pipe, read_from_pipe
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink
from ArmUtil import ToArmCoord, PSFunc
import PandaController
from controller import PositionSensor
armChain = Chain.from_urdf_file("panda_with_bound.urdf")
supervisor = Supervisor()
# create the Robot instance.
env = endpoint_env.EndP_env3D(supervisor)
env = preprocessing.RobotPreprocessing(env)
# rl = DDPG(2, 9, [-1, 1])
# get the time step of the current world.
timestep = env.timestep
# Environment Reset
def reset():
env.reset()
le_arm_chain = Chain(name='le_arm', active_links_mask=active_links_mask, links=[
URDFLink(
name="link6",
translation_vector=link6 * scale,
orientation=[0, 0, 0],
rotation=rotation6,
bounds=bounds6
),
URDFLink(
name="link5",
translation_vector=link5 * scale,
orientation=[0, 0, 0],
rotation=rotation5,
bounds=bounds5
),
URDFLink(
name="link4",
translation_vector=link4 * scale,
orientation=[0, 0, 0],
rotation=rotation4,
bounds=bounds4
),
URDFLink(
name="link3",
translation_vector=link3 * scale,
orientation=[0, 0, 0],
rotation=rotation3,
bounds=bounds3
),
URDFLink(
name="link2",
translation_vector=link2 * scale,
orientation=[0, 0, 0],
rotation=rotation2,
bounds=bounds2
),
URDFLink(
name="link1",
translation_vector=link1 * scale,
orientation=[0, 0, 0],
rotation=rotation1,
bounds=bounds1
)
])
def __init__(self, version = 'full'):
"""Instanciates both a pypot.Robot and a ikpy.Chain corresponding to the version of the Poppy Gripper Arm.
:param string version: Optional: Version of the robot to instanciate.
'noclamp' corresponds to a version whithout the gripper motors and clamps, and the endpoint should be placed at the end of a stick fixed to the forearm
Any other parameter corresponds to a full version where every motor is enabled and the endpoint is placed along the wrist's rotation axis, at the level of the grasping area
:returns: The list of angular positions
"""
self.version = 'full' if version != 'noclamp' else 'noclamp'
sleep(0.5) # Short pause to allow the robot connections to fully load
# Import URDF to build the kinematic chain
if version == 'noclamp':
URDFpath = URDFpath_noclamp
JSONpath = JSONpath_noclamp
else:
URDFpath = URDFpath_full
JSONpath = JSONpath_full
self.robot = PoppyRightGripper(config = JSONpath)
self.chain = Chain.from_urdf_file(URDFpath, active_links_mask = [False, True, True, True, True, False, False])
# Change PID of arm motors MX-28
for m in filter(lambda m: hasattr(m, 'pid'), self.robot.r_arm):
m.pid = (1.9, 5, 0)
if self.version != 'noclamp':
# Change PID of wrist motor XL-320
self.robot.r_wrist_z.pid = (4, 2, 0)
# Change PID of gripper motor XL-320
self.robot.r_gripper.pid = (8, 0, 0)
# Reduce max torque to keep motor temperature low
for m in self.robot.motors:
m.torque_limit = 70
# Calculate the shoulder position and the total arm length to define the sphere including all the reachable space
self.shoulder_origin = np.ravel( np.dot(self.chain.links[1].get_transformation_matrix(0), self.chain.links[2].get_transformation_matrix(0) )[:3, 3])
self.length = sum([l._length for l in self.chain.links[3:] ]) * 0.99 # Rough approximation of the arm's length
# Define the regularization parameters corresponding to each link: factor and ideal angle
reg_factor = 0.01 * np.array([0, 5, 4.5, 1.8, 1, 0, 0])
reg_angles = np.radians([0., 0., 0., 0., -80., 0., 0.])
self.regularization_parameters = (reg_factor, reg_angles)
# Create a new list of motors in order to sync ikpy Links with pypot Motors along the kinematic chain.
self.motors = []
motors_names = [m.name for m in self.robot.motors]
nMotors = len(motors_names) if self.version == 'noclamp' else len(motors_names) - 1 # The clamp motor is ignored in full version
for l in self.chain.links[1:(nMotors + 1)]: # The base link isn't taken into account
try:
self.motors.append(self.robot.motors[motors_names.index(l.name)])
except (ValueError):
raise ValueError('Failed to find a motor named "{}". Check for discrepancies between\n{} and\n{}'.format(l.name, JSONpath, URDFpath) )
# Import the angular limits of the motors
self.lower_limits = [min(m.lower_limit, m.upper_limit) for m in self.motors]
self.upper_limits = [max(m.lower_limit, m.upper_limit) for m in self.motors]
# Set at standard starting position
self.robot.compliant = False
self.robot.power_up()
self.init_angles()
print('Gripper arm initialized and running...')
