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 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]
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
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)
]
robo_chain = Chain(name="robo_chain", links=ulinks)
robo_chain.forward_kinematics([0] * len(ulinks))
# test matrix, to check if defaults are as expected:
look_straight = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
# translate in all directions
tf1 = [[1, 0, 0, 2], [0, 1, 0, 2], [0, 0, 1, 2], [0, 0, 0, 1]]
# rotate along all axes
tf2 = [[0.7, 0, 0, 2], [0, 0.7, 0, 2], [0, 0, 0.7, 2], [0, 0, 0, 1]]
#robo__chain.inverse_kinematics(look_straight)
import time
#time.sleep(1000)
if __name__ == '__main__':
# show inverse kinematic
import matplotlib.pyplot
from mpl_toolkits.mplot3d import Axes3D
for tf in [look_straight, tf1, tf2]:
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
robo_chain.plot(robo_chain.inverse_kinematics(tf), ax)
matplotlib.pyplot.show()
# close current plot, to show the next!
def ik(self, target_position=None, target_orientation=None, orientation_mode=None, **kwargs):
full = Chain.inverse_kinematics(self, target_position, target_orientation, orientation_mode, **kwargs)
active = self.joints_from_links(full)
return active
URDFLink(
name="EOF",
translation_vector=[0.5, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
# bounds = (0, 0)
)
])
# blue
#
target_vector = [0.4, 0.8, .2]
target_frame = np.eye(4)
target_frame[:3, 3] = target_vector
angles = my_chain.inverse_kinematics(target_frame)
for (i, angle) in enumerate(angles):
angles[i] = math.degrees(angle)
print("angle({}) = {} deg, {} pos".format(i, angles[i],
int(angles[i] / 0.24)))
real_frame = my_chain.forward_kinematics(
my_chain.inverse_kinematics(target_frame))
print("Computed position vector : %s, original position vector : %s" %
(real_frame[:3, 3], target_frame[:3, 3]))
import matplotlib.pyplot as plt
ax = plot_utils.init_3d_figure()
my_chain.plot(my_chain.inverse_kinematics(target_frame),
URDFLink(
name="joint3",
translation_vector=[0.045, 0, 0.802],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
),
URDFLink(
name="joint4",
translation_vector=[-0.045, 0, 0.746],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
)
])
print("start")
angle = my_chain.inverse_kinematics([
[1, 0, 0, 0],
[0, 1, 0, 0.3],
[0, 0, 1, 1],
[0, 0, 0, 1]
])
print(angle)
angle = my_chain.inverse_kinematics([
[1, 0, 0, 0],
[0, 1, 0, 0.5],
[0, 0, 1, 1],
[0, 0, 0, 1]
])
print(angle)
# [0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 1,1,1,1,1], # LL
# [0,0,0,0,0, 1,1,1,1,1, 0,0,0,0,0, 0,0,0,0,0], # RL
# [0,0,0,0,0, 0,0,0,0,0, 1,1,1,1,1, 0,0,0,0,0], # LA
# [1,1,1,1,1, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0], # RA
# Phase shift between arms/legs, [RA, RL, LA, LL]
shiftFrame = np.round(np.array([0, pi / 2, pi, 3 * pi / 2]) / 2 / pi * frames)
#shiftFrame = np.round(np.array([0, 0, pi, pi])/2/pi*frames)
#shiftFrame = np.round(np.array([0, pi, pi, 0])/2/pi*frames)
shiftFrame = shiftFrame.astype(int)
for frame in range(0, frames):
right_arm_angles_raw = right_arm.inverse_kinematics(
target_position=np.array([
stepLength * longitudinalMovement[frame], -bodyWidth /
2, swingHeight * verticalMovement[frame]
]))
right_arm_correction = np.array([0, -pi / 2, pi / 2, 0])
right_arm_angles = np.round(rad2deg *
(right_arm_angles_raw + right_arm_correction))
right_arm_angles = np.delete(right_arm_angles, np.s_[0, 3], axis=0)
right_arm_angles = right_arm_angles.astype(int)
right_leg_angles_raw = right_leg.inverse_kinematics(
target_position=np.array([
-bodyLength + stepLength *
longitudinalMovement[frame + shiftFrame[1]], -bodyWidth /
2, swingHeight * verticalMovement[frame + shiftFrame[1]]
]))
right_leg_correction = np.array([0, 0, -pi / 2, -pi / 2, 0])
# take the coordinate of the targer
#
x_input = float(input("X: "))
y_input = float(input("Y: "))
z_input = float(input("Z: "))
# initiate the target vector and the target frame
#
x = x_input #* SCALER
y = y_input #* SCALER
z = z_input #* SCALER
target_vector = [x, y, z]
target_frame = np.eye(4)
target_frame[:3, 3] = target_vector
angles = arm.inverse_kinematics(target_frame)
for (i, angle) in enumerate(angles):
angles[i] = math.degrees(angle)
if (i == 0):
angles[i] = angles[i] + 30
elif (i == 3 and angles[i] == 0): # FIXME: try and fix this
angles[i] = angles[i] + 180
# if (angles[i] > 360 ):
# angles[i] = angles[i] % 360
print("angle({}) = {} deg, {} pos".format(i, angles[i],
int(angles[i] / 0.24)))
name="joint1",
translation_vector=[0, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
),
URDFLink(
name="joint2",
translation_vector=[0, 0, 0.075],
orientation=[0, 0, 0],
rotation=[1, 0, 0],
),
URDFLink(
name="joint3",
translation_vector=[0.045, 0, 0.802],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
),
URDFLink(
name="joint4",
translation_vector=[-0.045, 0, 0.746],
orientation=[0, 0, 0],
rotation=[0, 0, 0],
)
])
my_chain.plot(
my_chain.inverse_kinematics([[1, 0, 0, -0.3], [0, 1, 0, 0.3], [0, 0, 1, 1],
[0, 0, 0, 1]]), ax)
#my_chain.plot([0,0,0],ax)
matplotlib.pyplot.show()
orientation=[0, 0, 0],
rotation=[0, 0, 1],
)
])
# positions = robotic_arm_chain.forward_kinematics([0] * 4)
# print("Positions: \n{}".format(positions))
#
# # target_vector = [0.1, -0.2, 0.1]
# target_vector = [0.5, 0.5, 0.0]
# target_frame = geometry_utils.to_transformation_matrix(
# target_vector,
# np.eye(3))
# print("The angles of each joints are : ", robotic_arm_chain.inverse_kinematics(target_frame))
# real_frame = robotic_arm_chain.forward_kinematics(robotic_arm_chain.inverse_kinematics(target_frame))
# print("Computed position vector : %s, original position vector : %s" % (real_frame[:3, 3], target_frame[:3, 3]))
#
#
# ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
# end = robotic_arm_chain.inverse_kinematics(target_frame)
# robotic_arm_chain.plot(end, ax, target=target_vector)
#
# # matplotlib.pyplot.xlim(-0.2, 0.2)
# # matplotlib.pyplot.ylim(-0.2, 0.2)
# matplotlib.pyplot.title("Target: {}, end: {}".format(target_vector, end))
# matplotlib.pyplot.show()
ik = robotic_arm_chain.inverse_kinematics([[1, 0, 0, 2], [0, 1, 0, 2],
[0, 0, 1, 2], [0, 0, 0, 1]])
print("ik: {}".format(ik))
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],
)
])
target_position = [2, 2, 2]
print(
left_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(target_position, np.eye(3))))
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
left_arm_chain.plot(left_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(
[2, 2, 2], [[1, 0, 0], [0, 1, 0], [0, 0, 1]])),
ax,
target=target_position)
matplotlib.pyplot.show()
class InverseKinematics():
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):
# # First robo, with MG995 Servos
# self.armLength = 10
# self.smallArmLength = 5
# 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=(-4*math.pi/9,4*math.pi/9)
# ),
# URDFLink(
# name="second",
# translation_vector=[0, 0, self.armLength],
# orientation=[0, 0, 0],
# rotation=[0, -1, 0],
# bounds=(-4*math.pi/9,4*math.pi/9)
# ),
# URDFLink(
# name="third",
# translation_vector=[0, 0, self.armLength],
# orientation=[0, 0, 0],
# rotation=[0, 1, 0],
# bounds=(-4*math.pi/9,4*math.pi/9)
# ),
# URDFLink(
# name="fourth",
# translation_vector=[0, 0, self.armLength],
# orientation=[0, 0, 0],
# rotation=[0, -1, 0],
# bounds=(-4*math.pi/9,4*math.pi/9)
# ),
# 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 __del__(self):
pass
def getPosition(self, angles, allLinks=False):
anglesRad = []
if not allLinks:
anglesRad.append(0)
for angle in angles:
anglesRad.append(angle * self.degreeToRadFactor)
if not allLinks:
anglesRad.append(0)
# print("anglesRad: {}".format(anglesRad))
calcPos = self.chain.forward_kinematics(anglesRad)[:3, 3]
calcPosPerc = []
calcPosPerc.append(
myutils.map(calcPos[0], self.xMin, self.xMax, -100, 100))
calcPosPerc.append(
myutils.map(calcPos[1], self.yMin, self.yMax, -100, 100))
calcPosPerc.append(
myutils.map(calcPos[2], self.zMin, self.zMax, 0, 100))
return calcPosPerc
def getAngles(self, pos):
if pos[0] == 0:
pos[0] = 0.01
if pos[1] == 0:
pos[1] = 0.01
if pos[2] == 0:
pos[2] = 0.01
pos[0] = myutils.map(pos[0], 0, 100, self.xMin, self.xMax)
pos[1] = myutils.map(pos[1], 0, 100, self.yMin, self.yMax)
pos[2] = myutils.map(pos[2], 0, 100, self.zMin, self.zMax)
anglesRad = self.chain.inverse_kinematics(pos)
angles = anglesRad * self.radToDegreeFactor
calcPos = self.chain.forward_kinematics(anglesRad)[:3, 3]
calcPosPerc = self.getPosition(angles, True)
newAngles = []
for i in range(1, self.numberOfLinks - 1):
# newAngles.append(math.floor(angles[i]))
newAngles.append(angles[i])
outOfRange = False
for i in range(len(calcPosPerc)):
if abs(calcPosPerc[i] - pos[i]) > 2:
outOfRange = True
break
log("{} calcPosPerc {}".format(outOfRange, calcPosPerc), "OK")
print("Angles are: {}".format(newAngles))
return newAngles, outOfRange
def getAnglesRaw(self, pos):
if pos[0] == 0:
pos[0] = 0.01
if pos[1] == 0:
pos[1] = 0.01
if pos[2] == 0:
pos[2] = 0.01
corrPos = []
corrPos.append(pos[0] * self.xMax / 100)
corrPos.append(pos[1] * self.yMax / 100)
corrPos.append(pos[2] * self.zMax / 100)
anglesRad = self.chain.inverse_kinematics(corrPos)
angles = anglesRad * self.radToDegreeFactor
startTime = time.time()
calcPos = self.chain.forward_kinematics(anglesRad)[:3, 3]
log("elapsed timeO: {}".format(time.time() - startTime))
calcPosPerc = self.getPosition(angles, True)
newAngles = []
for i in range(1, self.numberOfLinks - 1):
newAngles.append(angles[i])
outOfRange = False
for i in range(len(calcPosPerc)):
if abs(calcPosPerc[i] - pos[i]) > 2:
outOfRange = True
break
log("all Angles: {}".format(angles))
log("Out of range: {}, calcPosPerc {}".format(outOfRange, calcPosPerc),
"OK")
log("set pos: {}".format(pos), "OK")
log("Angles are: {}".format(newAngles))
return newAngles, outOfRange
rotation=[0, 0, 0],
)
],
active_links_mask=[False, True, True, True])
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
target_vector = [0.1, -0.2, 0.1]
l_wrist_matrix = geometry_utils.to_transformation_matrix(target_vector)
target_frame = np.eye(4)
target_frame[:3, 3] = target_vector
left_arm_start_position = left_arm_chain.forward_kinematics([0] * 4)
right_arm_start_position = right_arm_chain.forward_kinematics([0] * 4)
head_start_position = head_chain.forward_kinematics([0] * 3)
left_leg_start_position = left_leg_chain.forward_kinematics([0] * 4)
right_leg_start_position = right_leg_chain.forward_kinematics([0] * 4)
print(right_arm_start_position)
left_arm_chain.plot(left_arm_chain.inverse_kinematics(l_wrist_matrix),
ax,
target=target_vector)
right_arm_chain.plot(
right_arm_chain.inverse_kinematics(right_arm_start_position), ax)
head_chain.plot(head_chain.inverse_kinematics(head_start_position), ax)
left_leg_chain.plot(left_leg_chain.inverse_kinematics(left_leg_start_position),
ax)
right_leg_chain.plot(
right_leg_chain.inverse_kinematics(right_leg_start_position), ax)
matplotlib.pyplot.show()
class Control:
"""
Realization of continuous actions, from world model to desired world.
"""
def __init__(self, init_world_model):
self.control_world_model = init_world_model.current_world_model.control
self.arm_url = init_world_model.current_world_model.url["arm"]
self.cm_to_servo_polynomial_fitter = \
joblib.load(init_world_model.current_world_model.control["cm_to_servo_polynomial_fitter"]["file_path"])
self.init_position = np.array(self.control_world_model["init_position"]
) * self.control_world_model["scale"]
self.link_bounds = tuple(
np.radians(
np.array([
-self.control_world_model["angle_degree_limit"],
self.control_world_model["angle_degree_limit"]
]))) # In degrees
self.le_arm_chain = Chain(
name=self.control_world_model["chain_name"],
active_links_mask=self.control_world_model["active_links_mask"],
links=[
URDFLink(
name="link6",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link6"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link6"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link6"]),
bounds=self.link_bounds),
URDFLink(
name="link5",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link5"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link5"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link5"]),
bounds=self.link_bounds),
URDFLink(
name="link4",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link4"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link4"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link4"]),
bounds=self.link_bounds),
URDFLink(
name="link3",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link3"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link3"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link3"]),
bounds=self.link_bounds),
URDFLink(
name="link2",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link2"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link2"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link2"]),
bounds=self.link_bounds),
URDFLink(
name="link1",
translation_vector=np.array(
self.control_world_model["link_lengths"]["link1"]) *
self.control_world_model["scale"],
orientation=self.control_world_model["link_orientations"]
["link1"],
rotation=np.array(
self.control_world_model["joint_rotation_axis"]
["link1"]),
bounds=self.link_bounds)
])
def xyz_to_servo_range(self, xyz, current_servo_monotony):
"""
Converts 3D cartesian centimeter coordinates to servo values in [500, 2500].
:param xyz: Array of 3 elements of a 3D cartesian systems of centimeters.
:param current_servo_monotony: List of 6 positive or negative servo rotation directions.
:return: List of 6 servo values in [500, 2500].
"""
k = self.le_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(xyz, np.eye(3)))
k = np.multiply(k, np.negative(current_servo_monotony))
return self.radians_to_servo_range(k)
def servo_range_to_xyz(self, servo_range, current_servo_monotony):
"""
Converts servo values in [500, 2500] to 3D cartesian centimeter coordinates.
:param servo_range: List of 6 servo values in [500, 2500].
:param current_servo_monotony: List of 6 positive or negative servo rotation directions.
:return: Array of 3 elements of a 3D cartesian systems of centimeters.
"""
return geometry_utils.from_transformation_matrix(
self.le_arm_chain.forward_kinematics(
np.multiply(self.servo_range_to_radians(servo_range),
np.negative(current_servo_monotony)), ))[0][:3]
@staticmethod
def servo_range_to_radians(x,
x_min=500.0,
x_max=2500.0,
scaled_min=(-np.pi / 2.0),
scaled_max=(np.pi / 2.0)):
"""
Converts servo values in [500, 2500] to angle radians.
:param x: List of 6 servo values.
:param x_min: Scalar float, minimum servo value of 90 degrees angle (default = 500).
:param x_max: Scalar float, maximum servo value of 90 degrees angle(default = 2500).
:param scaled_min: Scalar float, minimum radians value of +90 degrees angle(default = -π/2).
:param scaled_max: Scalar float, maximum radians value of +90 degrees angle(default = π/2).
:return: List of 6 angles in radians.
"""
x_std = (np.array(x) - x_min) / (x_max - x_min)
return x_std * (scaled_max - scaled_min) + scaled_min
@staticmethod
def radians_to_servo_range(x,
x_min=(-np.pi / 2.0),
x_max=(np.pi / 2.0),
scaled_min=500.0,
scaled_max=2500.0):
"""
Converts angle radians to servo values in [500, 2500].
:param x: List of 6 angles in radians.
:param x_min: Scalar float, minimum radians value of +90 degrees angle(default = -π/2).
:param x_max: Scalar float, maximum radians value of +90 degrees angle(default = π/2).
:param scaled_min: Scalar float, minimum servo value of 90 degrees angle (default = 500).
:param scaled_max: Scalar float, maximum servo value of 90 degrees angle(default = 2500).
:return: List of 6 servo values.
"""
x_std = (np.array(x) - x_min) / (x_max - x_min)
return (np.round(x_std * (scaled_max - scaled_min) + scaled_min,
0)).astype(int)
def get_kinematic_angle_trajectory(self,
from_angle_radians_in,
to_angle_radians_in,
servo_monotony,
steps=10):
"""
Creates a discrete end-effector trajectory, using radians.
:param from_angle_radians_in: Current servo angles, list of 6 angles in radians.
:param to_angle_radians_in: Desired servo angles, list of 6 angles in radians.
:param servo_monotony: List of 6 positive or negative servo rotation directions.
:param steps: Scalar integer, the total steps for the end effector trajectory.
:return: List of end-effector radian trajectory steps.
"""
assert self.control_world_model[
"min_steps"] < steps < self.control_world_model["max_steps"]
from_angle_radians = np.multiply(from_angle_radians_in, servo_monotony)
to_angle_radians = np.multiply(to_angle_radians_in, servo_monotony)
step_angle_radians = []
for index in range(len(to_angle_radians_in)):
step_angle_radians.append(
(from_angle_radians[index] - to_angle_radians[index]) /
float(steps))
angle_trajectory = []
step_angle_radians = np.array(step_angle_radians)
current_angles = np.array(from_angle_radians)
# angle_trajectory.append(current_angles)
for _ in range(steps):
current_angles = np.add(current_angles, step_angle_radians)
angle_trajectory.append(current_angles)
return angle_trajectory
def get_servo_range_trajectory(self,
from_servo_range_in,
to_servo_range_in,
steps=10):
"""
Creates a discrete end-effector trajectory, using servo values.
:param from_servo_range_in: Current servo values, list of 6 values in [500, 2500].
:param to_servo_range_in: Desired servo values, list of 6 values in [500, 2500].
:param steps: Scalar integer, the total steps for the end effector trajectory.
:return: List of end-effector servo value trajectory steps.
"""
assert self.control_world_model[
"min_steps"] < steps < self.control_world_model["max_steps"]
from_servo_range = np.array(from_servo_range_in)
to_servo_range = np.array(to_servo_range_in)
if self.control_world_model["verbose"]:
print("from_servo_range: ", from_servo_range)
print("to_servo_range: ", to_servo_range)
step_servo_range = []
for index in range(len(to_servo_range)):
step_servo_range.append(
(to_servo_range[index] - from_servo_range[index]) /
float(steps))
if self.control_world_model["verbose"]:
print("step_servo_range: ", step_servo_range)
servo_range_trajectory = []
step_servo_range = np.array(step_servo_range)
current_servo_range = np.array(from_servo_range)
for _ in range(steps):
current_servo_range = np.add(current_servo_range, step_servo_range)
servo_range_trajectory.append(current_servo_range)
return np.array(np.round(servo_range_trajectory, 0)).astype(int)
def initialize_arm(self, last_servo_values):
"""
Moves the end-effector to the (0, 0, 0) position of the 3d cartesian.
:param last_servo_values: List of the current arm servo positions.
:return: True if succeeded.
"""
action_successful = False
target_position = np.array(
self.init_position) * self.control_world_model["scale"]
action_successful = self.move_arm(target_position, last_servo_values)
print("=== Arm initialized")
return action_successful
def move_arm_above_xyz(self, xyz, last_servo_values, height):
"""
Moves the end-effector at a specific 3D cartesian centimeter position, plus extra centimeters high.
:param xyz: Array of 3 elements of a 3D cartesian systems of centimeters.
:param last_servo_values: List of the current arm servo positions.
:param height: Scalar positive float. Desired centimeters above xyz, on the z axis.
:return: True if succeeded.
"""
action_successful = False
xyz[2] = height
target_position = np.array(xyz) * self.control_world_model["scale"]
if self.control_world_model["send_requests"]:
action_successful = self.move_arm(target_position,
last_servo_values)
print("=== Arm above object")
return action_successful
def move_arm_up(self, last_servo_values, height):
"""
Moves the end-effector at a specific 3D cartesian centimeter position, plus extra centimeters high.
:param last_servo_values: List of the current arm servo positions.
:param height: Scalar positive float. Desired centimeters above xyz, on the z axis.
:return: True if succeeded.
"""
action_successful = False
xyz = np.round(
self.servo_range_to_xyz(
last_servo_values,
self.control_world_model["current_servo_monotony"]), 2)
print("last_servo_xyz", xyz)
xyz[2] = height
target_position = np.array(xyz) * self.control_world_model["scale"]
if self.control_world_model["send_requests"]:
action_successful = self.move_arm(target_position,
last_servo_values)
print("=== Arm up")
return action_successful
def move_arm_to_object(self, xyz, last_servo_values):
"""
Moves the end-effector to the object's position of the 3d cartesian.
:param xyz: Array of 3 elements of a 3D cartesian systems of centimeters.
:param last_servo_values: List of the current arm servo positions.
:return: True if succeeded.
"""
action_successful = False
target_position = np.array(xyz) * self.control_world_model["scale"]
if self.control_world_model["send_requests"]:
action_successful = self.move_arm(target_position,
last_servo_values)
print("=== Arm to object")
return action_successful
def close_hand(self, object_side_length):
"""
Closes the gripper enough, to grip an object of a specific length in cm.
:param object_side_length: Scalar float, object width in centimeters.
:return: True if succeeded.
"""
action_successful = False
closed_length = object_side_length * self.control_world_model[
"closed_hand_distance_ratio"]
servo_range = int(self.cm_to_servo_polynomial_fitter(closed_length))
if self.control_world_model["verbose"]:
print("cm: {}, predicted servo value: {}".format(
closed_length, servo_range))
if self.control_world_model["send_requests"]:
action_successful = self.send_restful_servo_range(
self.control_world_model["gripping_gripper_servo"],
servo_range)
print("=== Gripper closed")
return action_successful
def open_hand(self, object_side_length):
"""
Opens the gripper enough, to fit an object of a specific length in cm.
:param object_side_length: Scalar float, object width in centimeters.
:return: True if succeeded.
"""
action_successful = False
opened_length = object_side_length * self.control_world_model[
"opened_hand_distance_ratio"]
servo_range = int(self.cm_to_servo_polynomial_fitter(opened_length))
if self.control_world_model["verbose"]:
print("cm: {}, predicted servo value: {}".format(
opened_length, servo_range))
if self.control_world_model["send_requests"]:
action_successful = self.send_restful_servo_range(
self.control_world_model["gripping_gripper_servo"],
servo_range)
print("=== Gripper opened")
return action_successful
def send_restful_servo_range(self, servo, in_range):
"""
Sends a direct servo value in [500, 2500], to a specific servo in [1, 6].
:param servo: Scalar integer, the servo id in [1, 6].
:param in_range: Scalar integer, servo value in [500, 2500].
:return: True if succeeded.
"""
action_successful = False
url = self.control_world_model["base_put_url"].format(
self.arm_url, servo, in_range)
requests.put(url, data="")
time.sleep(self.control_world_model["command_delay"])
action_successful = True
return action_successful
def send_restful_trajectory_requests(self,
kinematic_servo_range_trajectory):
"""
Sends a full servo value trajectory of discrete steps, to the arm.
:param kinematic_servo_range_trajectory:
:return: True if succeeded.
"""
action_successful = False
servo_mask = self.control_world_model[
"active_links_mask"] # servo mask
for step in kinematic_servo_range_trajectory:
for i in range(len(step)):
if servo_mask[i]:
servo_value = step[i]
current_servo = self.control_world_model["servo_count"] - i
url = self.control_world_model["base_put_url"].format(
self.arm_url, current_servo, servo_value)
if self.control_world_model["verbose"]:
print(url)
try:
r = requests.put(url, data="")
if r.status_code != 200:
break
except Exception as e:
print("Exception: {}".format(str(e)))
time.sleep(self.control_world_model["command_delay"])
if self.control_world_model["verbose"]:
print("")
action_successful = True
return action_successful
def move_arm(self,
target_position,
last_servo_locations,
trajectory_steps=-1):
"""
Gradually moves the end-effector of the robotic arm, from the latest known servo positions, to a desired
3D centimeter cartesian position.
:param target_position: List of 3 values, the desired end-effector, 3D centimeter cartesian position.
:param last_servo_locations: List of the latest 6 servo values in [500, 2500].
:param trajectory_steps: Scalar integer, the total steps for the end effector trajectory.
:return: True if successfully move the arm.
"""
action_successful = False
last_servo_values = self.init_position
if trajectory_steps == -1:
trajectory_steps = self.control_world_model["trajectory_steps"]
# TODO: move last position to world model
if self.control_world_model[
"detect_last_position"]: # TODO: get last servo values, world may be old...?
last_servo_values = last_servo_locations
try:
if self.control_world_model["send_requests"]:
url = "http://{}/".format(self.arm_url)
r = requests.get(url, data="")
if r.status_code == 200:
result = r.json()["variables"]
last_servo_values = np.array([
result["servo6"], result["servo5"],
result["servo4"], result["servo3"],
result["servo2"], result["servo1"]
])
if self.control_world_model["verbose"]:
print("last_servo_values: ", last_servo_values)
print(
"last_servo_xyz",
np.round(
self.servo_range_to_xyz(
last_servo_values,
self.control_world_model[
"current_servo_monotony"]), 2))
except Exception as e_pos:
print("Exception: {}".format(str(e_pos)))
if self.control_world_model["center_init"]:
if self.control_world_model["verbose"]:
print(
"Top position (radians): ",
self.le_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(
self.init_position, np.eye(3))))
if self.control_world_model["show_plots"]:
ax = matplotlib.pyplot.figure().add_subplot(111,
projection='3d')
self.le_arm_chain.plot(self.le_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(
self.init_position, np.eye(3))),
ax,
target=self.init_position)
matplotlib.pyplot.show()
init_position2 = self.init_position
init_angle_radians2 = self.le_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(init_position2, np.eye(3)))
from_servo_range = self.radians_to_servo_range(init_angle_radians2)
if self.control_world_model["detect_last_position"]:
from_servo_range = last_servo_values
to_servo_range = self.xyz_to_servo_range(
target_position,
self.control_world_model["current_servo_monotony"])
kinematic_servo_range_trajectory = self.get_servo_range_trajectory(
from_servo_range, to_servo_range, trajectory_steps)
if self.control_world_model["verbose"]:
print(
"init_angle_radians2: {}, from_servo_range: {}, to_servo_range: {}, servo_range_trajectory: {}"
.format(init_angle_radians2, from_servo_range, to_servo_range,
kinematic_servo_range_trajectory))
if self.control_world_model["show_plots"]:
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
self.le_arm_chain.plot(self.le_arm_chain.inverse_kinematics(
geometry_utils.to_transformation_matrix(
target_position, np.eye(3))),
ax,
target=target_position)
matplotlib.pyplot.show()
if self.control_world_model["send_requests"]:
action_successful = self.send_restful_trajectory_requests(
kinematic_servo_range_trajectory)
return action_successful
# Get the arm and target nodes.
target = supervisor.getFromDef('TARGET')
arm = supervisor.getSelf()
# Loop 1: Draw a circle on the paper sheet.
print('Draw a circle on the paper sheet...')
while supervisor.step(timeStep) != -1:
t = supervisor.getTime()
# Use the circle equation relatively to the arm base as an input of the IK algorithm.
x = 0.25 * math.cos(t) + 1.1
y = 0.25 * math.sin(t) - 0.95
z = 0.23
# Call "ikpy" to compute the inverse kinematics of the arm.
ikResults = armChain.inverse_kinematics([[1, 0, 0, x], [0, 1, 0, y],
[0, 0, 1, z], [0, 0, 0, 1]])
# Actuate the 3 first arm motors with the IK results.
for i in range(3):
motors[i].setPosition(ikResults[i + 1])
# Keep the hand orientation down.
motors[4].setPosition(-ikResults[2] - ikResults[3] + math.pi / 2)
# Keep the hand orientation perpendicular.
motors[5].setPosition(ikResults[1])
# Conditions to start/stop drawing and leave this loop.
if supervisor.getTime() > 2 * math.pi + 1.5:
break
elif supervisor.getTime() > 1.5:
# Note: start to draw at 1.5 second to be sure the arm is well located.
supervisor.getPen('pen').write(True)
# Get the arm and target nodes.
target = supervisor.getFromDef('TARGET')
arm = supervisor.getSelf()
# add camera
print('start find work space')
# Loop 2: Move the arm hand to the target.
print('Move the yellow and black sphere to move the arm...')
while supervisor.step(timeStep) != -1:
# Get the absolute postion of the target and the arm base.
targetPosition = target.getPosition()
armPosition = arm.getPosition()
print(targetPosition)
# Compute the position of the target relatively to the arm.
# x and y axis are inverted because the arm is not aligned with the Webots global axes.
x = targetPosition[0] - armPosition[0]
y = -(targetPosition[2] - armPosition[2])
z = targetPosition[1] - armPosition[1] + 0.5
# Call "ikpy" to compute the inverse kinematics of the arm.
ikResults = armChain.inverse_kinematics([x, y, z])
# Actuate the 3 first arm motors with the IK results.
for i in range(3):
motors[i].setPosition(ikResults[i + 1])
# y +- 0.7
# x +- 0.3
# print("init_servo_radians: ", init_servo_radians)
#
# print("init_servo_radians: ", np.multiply(servo_range_to_radians(init_servo_values),
# current_servo_monotony[::-1]))
#
# init_position2 = le_arm_chain.forward_kinematics(init_servo_radians)
# init_position = np.round(servo_range_to_xyz2(init_servo_values, current_servo_monotony), 2)
# print("predicted_init_position: ", init_position)
# print("proper init_position: ", target_position)
except Exception as e:
print("Exception: {}".format(str(e)))
if center_init:
print("Top position (radians): ", le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix(
init_position,
np.eye(3))))
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
le_arm_chain.plot(le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix(
init_position,
np.eye(3))), ax, target=init_position)
matplotlib.pyplot.show()
print("Target angles (radians): ", le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix(
target_position,
np.eye(3))))
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
le_arm_chain.plot(le_arm_chain.inverse_kinematics(geometry_utils.to_transformation_matrix(
target_position,
orientation=[0, 0, 0],
rotation=[0, 0, 1],
),
URDFLink(
name="second_link",
translation_vector=[1, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 0, 1],
)
])
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
target_vector = [1, 0, 0]
target_frame = np.eye(4)
target_frame[:3, 3] = target_vector
target_frame
print("The angles of each joints are : ",
test_link.inverse_kinematics(target_frame))
real_frame = test_link.forward_kinematics(
test_link.inverse_kinematics(target_frame))
print("--> Computed position vector : %s" % real_frame[:3, 3])
print("--> original position vector : %s" % target_frame[:3, 3])
ax = plot_utils.init_3d_figure()
test_link.plot(test_link.inverse_kinematics(target_frame),
ax,
target=target_vector)
matplotlib.pyplot.xlim(-1, 2)
matplotlib.pyplot.ylim(-1, 2)
matplotlib.pyplot.show()
