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
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]])
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)))
arm.plot(arm.inverse_kinematics(target_frame), ax, target=target_vector)
# check if reached !
#
real_frame = arm.forward_kinematics(arm.inverse_kinematics(target_frame))
print("Computed position vector : %s, original position vector : %s" %
(real_frame[:3, 3], target_frame[:3, 3]))
# plot
#
# plt.xlim(-SCALER, SCALER)
# plt.ylim(-SCALER, SCALER)
plt.show()
# sleep(2)
# print("showing moving arm")
# new_target_vector = [x, y, 0]
# target_frame[:3, 3] = new_target_vector
# copy target vector
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
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],
)
]
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)
def fk(self, joints, full_kinematics=False):
joints = np.array([0, 0, *joints, 0, 0, 0])
return Chain.forward_kinematics(self, joints, full_kinematics)
)
])
# 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),
ax,
target=target_vector)
print("-------yellow--------")
# yellow
#
mychain = Chain(
name='neck',
links=[
#OriginLink(),
URDFLink(
name="shoulder",
translation_vector=[0, 0, 0.065],
orientation=[-0.5 * PI, 0, 0],
rotation=[0, 0, 1],
)
# URDFLink(
# name="elbow",
# translation_vector=[0, 0, 0.129],
# orientation=[-PI*(156/180.0)+theta2, 0, 0],
# rotation=[0, 0, 1],
# ),
# URDFLink(
# name="wrist",
# translation_vector=[0, 0, 0.065],
# orientation=[(99/180.0)*PI+theta3, 0, 0],
# rotation=[0, 0, 1],
# ),
# URDFLink(
# name="head",
# translation_vector=[0, 0, 0.083],
# orientation=[(61/180.0)*PI+theta4, 0, 0],
# rotation=[0, 0, 1],
# )
])
print(mychain.forward_kinematics([0] * 1))
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
URDFLink(
name="ankle",
translation_vector=[0, 0, -1.13],
orientation=[0, 0, 0],
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)
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()
