import ikpy.utils.plot as plot_utils
import matplotlib.pyplot as plt
import numpy as np
left_arm_chain = Chain(name='left_arm',
links=[
OriginLink(),
URDFLink(
name="shoulder",
translation_vector=[-10, 0, 5],
orientation=[0, 1.57, 0],
rotation=[0, 1, 0],
),
URDFLink(
name="elbow",
translation_vector=[25, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
),
URDFLink(
name="wrist",
translation_vector=[22, 0, 0],
orientation=[0, 0, 0],
rotation=[0, 1, 0],
)
])
fig, ax = plot_utils.init_3d_figure()
left_arm_chain.plot([0, 0, 0, 0], ax)
plt.show()
# 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,
np.eye(3))), ax,
target=target_position)
matplotlib.pyplot.show()
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)))
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")
gait_sequence[[0, 2, 4, 6, 1, 3, 5, 7]],
fmt='%3.1i',
delimiter=',',
newline=", ")
#f.write("+")
f.write("\n")
f.close()
# Create plot and image for each frame
fig = plt.figure()
ax = p3.Axes3D(fig)
ax.set_box_aspect([3, 3 / 2, 1])
ax.set_xlim3d([-20, 10])
ax.set_xlabel('X')
ax.set_ylim3d([-10, 10])
ax.set_ylabel('Y')
ax.set_zlim3d([0.0, 10])
ax.set_zlabel('Z')
right_arm.plot(right_arm_angles_raw, ax)
right_leg.plot(right_leg_angles_raw, ax)
left_arm.plot(left_arm_angles_raw, ax)
left_leg.plot(left_leg_angles_raw, ax)
figureName = "Nybble_" + str(frame)
plt.savefig(figureName)
plt.close()
#plt.show()
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
#
target_vector = [0.4, 0.8, 0]
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)))
]
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!
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
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()
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()
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()
class Wobbler(object):
def __init__(self):
self._done = False
self._head = baxter_interface.Head()
self._left_arm = baxter_interface.limb.Limb('left')
self._right_arm = baxter_interface.limb.Limb('right')
print(self._left_arm.joint_names())
print(self._left_arm.joint_angles())
print(self._left_arm.joint_velocities())
print(self._left_arm.joint_efforts())
print(self._left_arm.endpoint_pose())
print(self._left_arm.endpoint_velocity())
print(self._left_arm.endpoint_effort())
self._left_arm_chain = Chain(
name='left_arm',
active_links_mask=[
False, False, True, True, True, True, True, True, True, False
],
links=[
OriginLink(),
URDFLink(
name="left_torso_arm_mount",
translation_vector=[0.024645, 0.219645, 0.118588],
orientation=[0, 0, 0.7854],
rotation=[0, 0, 1],
),
URDFLink(name="left_s0",
translation_vector=[0.055695, 0, 0.011038],
orientation=[0, 0, 0],
rotation=[0, 0, 1],
bounds=(-1.70167993878, 1.70167993878)),
URDFLink(name="left_s1",
translation_vector=[0.069, 0, 0.27035],
orientation=[-1.57079632679, 0, 0],
rotation=[0, 0, 1],
bounds=(-2.147, 1.047)),
URDFLink(name="left_e0",
translation_vector=[0.102, 0, 0],
orientation=[1.57079632679, 0, 1.57079632679],
rotation=[0, 0, 1],
bounds=(-3.05417993878, 3.05417993878)),
URDFLink(name="left_e1",
translation_vector=[0.069, 0, 0.26242],
orientation=[-1.57079632679, -1.57079632679, 0],
rotation=[0, 0, 1],
bounds=(-0.05, 2.618)),
URDFLink(name="left_w0",
translation_vector=[0.10359, 0, 0],
orientation=[1.57079632679, 0, 1.57079632679],
rotation=[0, 0, 1],
bounds=(-3.059, 3.059)),
URDFLink(name="left_w1",
translation_vector=[0.01, 0, 0.2707],
orientation=[-1.57079632679, -1.57079632679, 0],
rotation=[0, 0, 1],
bounds=(-1.57079632679, 2.094)),
URDFLink(name="left_w2",
translation_vector=[0.115975, 0, 0],
orientation=[1.57079632679, 0, 1.57079632679],
rotation=[0, 0, 1],
bounds=(-3.059, 3.059)),
URDFLink(
name="left_hand",
translation_vector=[0, 0, 0.11355],
orientation=[0, 0, 0],
rotation=[0, 0, 1],
)
])
#self._left_arm_chain = ikpy.chain.Chain.from_urdf_file("/home/jbunker/ros_ws/src/baxter_common/baxter_description/urdf/baxter.urdf")
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
def clean_shutdown(self):
"""
Exits example cleanly by moving head to neutral position and
maintaining start state
"""
print("\nExiting example...")
if self._done:
self.set_neutral()
if not self._init_state and self._rs.state().enabled:
print("Disabling robot...")
self._rs.disable()
def set_neutral(self):
"""
Sets the head back into a neutral pose
"""
self._head.set_pan(0.0)
def testJoint(self):
self.set_neutral()
print(self._left_arm.joint_names())
target = (self._right_arm.joint_names())[0]
print("targeting joint {0}".format(target))
print("starting angle {0}".format(self._right_arm.joint_angle(target)))
print("starting velocity {0}".format(
self._right_arm.joint_velocity(target)))
print("starting effort {0}".format(
self._right_arm.joint_effort(target)))
command_rate = rospy.Rate(1)
control_rate = rospy.Rate(50)
commandDictLeft = {}
for n in self._left_arm.joint_names():
commandDictLeft[n] = 0.
commandDictRight = {}
for n in self._right_arm.joint_names():
commandDictRight[n] = 0.
start = rospy.get_time()
while not rospy.is_shutdown() and (rospy.get_time() - start < 0.1):
self._left_arm.set_joint_positions(commandDictLeft)
self._right_arm.set_joint_positions(commandDictRight)
control_rate.sleep()
print("zeroing ended, recording starting waypoint...")
i = 0
while (i < 3):
state = jointStates.get()
print(i)
i += 1
originP = [0.064027, 0.259027, 0.08]
target = np.asarray([[1, 0, 0, .7], [0, 1, 0, .0], [0, 0, 1, .5],
[0, 0, 0, 1]])
angles = self._left_arm.joint_angles()
order = [
"origin", "left_torso_arm_mount", "left_s0", "left_s1", "left_e0",
"left_e1", "left_w0", "left_w1", "left_w2", "left_hand"
]
angles = properOrder(self._left_arm.joint_angles(), order)
print(angles)
print(target)
print(self._left_arm_chain)
solution = ikpy.inverse_kinematics.inverse_kinematic_optimization(
chain=self._left_arm_chain,
target_frame=target,
starting_nodes_angles=angles)
print(solution)
inp = convertToDict(order, solution)
i = 0
while (getDiff(angles, solution) > 0.01 and i < 1000):
print(getDiff(angles, solution))
inp = convertToDict(order, solution)
print(inp)
self._left_arm.set_joint_positions(convertToDict(order, solution))
#self._left_arm.set_joint_positions(commandDictLeft)
control_rate.sleep()
angles = properOrder(self._left_arm.joint_angles(), order)
#print(angles)
i += 1
print(self._left_arm_chain)
ax = matplotlib.pyplot.figure().add_subplot(111, projection='3d')
self._left_arm_chain.plot(
ikpy.inverse_kinematics.inverse_kinematic_optimization(
chain=self._left_arm_chain,
target_frame=target,
starting_nodes_angles=angles), ax)
matplotlib.pyplot.show()
#t_angles = self.left_arm_chain.inverse_kinematics(target=target_frame,initial_position=self._left_arm.joint_positions()))
self._done = True
rospy.signal_shutdown("Example finished.")
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],
)
])
left_arm_chain.plot(left_arm_chain.joints, ax)
# left_arm_chain.plot(left_arm_chain.inverse_kinematics([
# [1, 0, 0, 2],
# [0, 1, 0, 2],
# [0, 0, 1, 2],
# [0, 0, 0, 1]
# ]), ax)
matplotlib.pyplot.show()
