def abb_irb6640_180_255_robot():
"""Return a Robot instance for the ABB IRB6640 180-255 robot"""
x = np.array([1, 0, 0])
y = np.array([0, 1, 0])
z = np.array([0, 0, 1])
a = np.array([0, 0, 0])
H = np.array([z, y, y, x, y, x]).T
P = np.array(
[0.78 * z, 0.32 * x, 1.075 * z, 0.2 * z, 1.1425 * x, 0.2 * x, a]).T
joint_type = [0, 0, 0, 0, 0, 0]
joint_min = np.deg2rad(np.array([-170, -65, -180, -300, -120, -360]))
joint_max = np.deg2rad(np.array([170, 85, 70, 300, 120, 360]))
p_tool = np.array([0, 0, 0])
R_tool = rox.rot([0, 1, 0], np.pi / 2.0)
return rox.Robot(H,
P,
joint_type,
joint_min,
joint_max,
R_tool=R_tool,
p_tool=p_tool)
def test_attached_collision_object(self):
expected_scene_1 = rospy.wait_for_message("/expected_planning_scene_1",
PlanningScene,
timeout=10)
scene_1 = rospy.wait_for_message('/planning_scene',
PlanningScene,
timeout=10)
self._assert_planning_scene_equal(expected_scene_1, scene_1)
rospy.sleep(rospy.Duration(1))
update_pose_proxy = rospy.ServiceProxy("update_payload_pose",
UpdatePayloadPose)
payload2_to_gripper_target_tf = self.tf_listener.lookupTransform(
"payload2", "payload2_gripper_target", rospy.Time(0))
req = UpdatePayloadPoseRequest()
req.header.frame_id = "vacuum_gripper_tool"
req.name = "payload2"
req.pose = rox_msg.transform2pose_msg(
payload2_to_gripper_target_tf.inv())
req.confidence = 0.5
res = update_pose_proxy(req)
assert res.success
expected_scene_2 = rospy.wait_for_message("/expected_planning_scene_2",
PlanningScene,
timeout=10)
scene_2 = rospy.wait_for_message('/planning_scene',
PlanningScene,
timeout=10)
self._assert_planning_scene_equal(expected_scene_2, scene_2)
world_to_fixture2_payload2_target_tf = self.tf_listener.lookupTransform(
"world", "fixture2_payload2_target", rospy.Time(0))
#Add in an offset to represent a final placement error
fixture2_payload2_target_to_payload2_tf = rox.Transform(
rox.rot([0, 0, 1], np.deg2rad(5)), [0.1, 0, 0],
"fixture2_payload2_target", "payload2")
req2 = UpdatePayloadPoseRequest()
req2.header.frame_id = "world"
req2.name = "payload2"
req2.pose = rox_msg.transform2pose_msg(
world_to_fixture2_payload2_target_tf *
fixture2_payload2_target_to_payload2_tf)
res2 = update_pose_proxy(req2)
assert res2.success
expected_scene_3 = rospy.wait_for_message("/expected_planning_scene_3",
PlanningScene,
timeout=10)
scene_3 = rospy.wait_for_message('/planning_scene',
PlanningScene,
timeout=10)
self._assert_planning_scene_equal(expected_scene_3, scene_3)
def test_q2rot():
rot_t=np.array([[-0.5057639,-0.1340537,0.8521928], \
[0.6456962,-0.7139224,0.2709081], \
[0.5720833,0.6872731,0.4476342]])
q = np.array([0.2387194, 0.4360402, 0.2933459, 0.8165967])
k, theta = rox.q2rot(q)
np.testing.assert_allclose(rox.rot(k, theta), rot_t, atol=1e-6)
def runTest(self):
#TODO: other joint types
#Home configuration (See Page 2-2 of Puma 260 manual)
puma = puma260b_robot()
pose = rox.fwdkin(puma, np.zeros(6))
np.testing.assert_allclose(pose.R, np.identity(3))
np.testing.assert_allclose(pose.p,
np.array([10, -4.9, 4.25]) * in_2_m,
atol=1e-6)
#Another right-angle configuration
joints2 = np.array([180, -90, -90, 90, 90, 90]) * np.pi / 180.0
pose2 = rox.fwdkin(puma, joints2)
np.testing.assert_allclose(pose2.R,
rox.rot([0, 0, 1], np.pi).dot(
rox.rot([0, 1, 0], -np.pi / 2)),
atol=1e-6)
np.testing.assert_allclose(pose2.p,
np.array([-0.75, 4.9, 31]) * in_2_m)
#Random configuration
joints3 = np.array([50, -105, 31, 4, 126, -184]) * np.pi / 180
pose3 = rox.fwdkin(puma, joints3)
pose3_R_t=np.array([[0.4274, 0.8069, -0.4076],\
[0.4455, -0.5804,-0.6817], \
[-0.7866, 0.1097, -0.6076]])
pose3_p_t = [0.2236, 0.0693, 0.4265]
np.testing.assert_allclose(pose3.R, pose3_R_t, atol=1e-4)
np.testing.assert_allclose(pose3.p, pose3_p_t, atol=1e-4)
puma_tool = puma260b_robot_tool()
pose4 = rox.fwdkin(puma_tool, joints3)
pose4_R_t=np.array([[0.4076, 0.8069, 0.4274],\
[0.6817, -0.5804,0.4455], \
[0.6076, 0.1097, -0.7866]])
pose4_p_t = [0.2450, 0.0916, 0.3872]
np.testing.assert_allclose(pose4.R, pose4_R_t, atol=1e-4)
np.testing.assert_allclose(pose4.p, pose4_p_t, atol=1e-4)
async def async_jog_cartesian_gamepad(P_axis, R_axis):
move_distance = 0.01 # meters
rotate_angle = np.deg2rad(5) # radians
global d, num_joints, joint_lower_limits, joint_upper_limits, joint_vel_limits
global pose # Get the Current Pose of the robot
global is_gamepadaxisactive
global is_gamepadbuttondown
# print_div("here 0<br>")
if (is_gamepadaxisactive or is_gamepadbuttondown):
# Update joint angles
d_q = await update_joint_info() # Joint angles in radian ndarray
# UPdate the end effector pose info
pose = await update_end_info()
await update_state_flags()
Rd = pose.R
pd = pose.p
# print_div("here 1<br>")
if P_axis is not None:
pd = pd + Rd.dot(move_distance * P_axis)
# print_div("here 2<br>")
if R_axis is not None:
# R = rox.rot(np.array(([1.],[0.],[0.])), 0.261799)
R = rox.rot(R_axis, rotate_angle)
Rd = Rd.dot(R) # Rotate
# print_div("here 3<br>")
try:
# Update desired inverse kineamtics info
joint_angles, converged = update_ik_info(Rd, pd)
if not converged:
print_div("Inverse Kinematics Algo. Could not Converge<br>")
raise
elif not (joint_angles < joint_upper_limits).all() or not (
joint_angles > joint_lower_limits).all():
print_div("Specified joints are out of range<br>")
raise
else:
await d.async_jog_joint(joint_angles, joint_vel_limits, False,
True, None)
except:
print_div("Specified joints might be out of range<br>")
global is_jogging
is_jogging = False
def compute_step_gripper_target_pose(self,
img,
Kp,
no_z=False,
z_offset=0):
fixed_marker_transform, payload_marker_transform, error_transform = self.compute_error_transform(
img)
if no_z:
error_transform.p[2] = 0
else:
error_transform.p[2] -= z_offset
gripper_to_camera_tf = self.tf_listener.lookupTransform(
"vacuum_gripper_tool", "gripper_camera_2", rospy.Time(0))
world_to_vacuum_gripper_tool_tf = self.tf_listener.lookupTransform(
"world", "vacuum_gripper_tool", rospy.Time(0))
#Scale by Kp
k, theta = rox.R2rot(error_transform.R)
r = np.multiply(k * theta, Kp[0:3])
r_norm = np.linalg.norm(r)
if (r_norm < 1e-6):
error_transform2_R = np.eye(3)
else:
error_transform2_R = rox.rot(r / r_norm, r_norm)
error_transform2_p = np.multiply(error_transform.p, (Kp[3:6]))
error_transform2 = rox.Transform(error_transform2_R,
error_transform2_p)
gripper_to_fixed_marker_tf = gripper_to_camera_tf * fixed_marker_transform
gripper_to_desired_fixed_marker_tf = gripper_to_fixed_marker_tf * error_transform2
#print gripper_to_fixed_marker_tf
ret = world_to_vacuum_gripper_tool_tf * (
gripper_to_desired_fixed_marker_tf *
gripper_to_fixed_marker_tf.inv()).inv()
#print world_to_vacuum_gripper_tool_tf
#print ret
#print error_transform
return ret, error_transform
def _R2rot_test(k1, theta1):
R = rox.rot(k1, theta1)
k2, theta2 = rox.R2rot(R)
if abs(theta1 - theta2) > (theta1 + theta2):
k2 = -k2
theta2 = -theta2
np.testing.assert_allclose(theta1, theta2, atol=1e-6)
if (abs(theta1) < 1e-9):
return
if ((np.abs(theta1) - np.pi) < 1e-9):
if np.linalg.norm(k1 + k2) < 1e-6:
np.testing.assert_allclose(k1, -k2, atol=1e-6)
return
np.testing.assert_allclose(k1, k2, atol=1e-6)
return
np.testing.assert_allclose(k1, k2, atol=1e-6)
def compute_step_gripper_target_pose(img, fixed_marker, payload_marker, desired_transform, \
camera_info, aruco_dict, Kp, tf_listener):
fixed_marker_transform, error_transform = compute_error_transform(img, fixed_marker, payload_marker, \
desired_transform, camera_info, aruco_dict)
gripper_to_camera_tf = tf_listener.lookupTransform("vacuum_gripper_tool",
"gripper_camera_2",
rospy.Time(0))
world_to_vacuum_gripper_tool_tf = tf_listener.lookupTransform(
"world", "vacuum_gripper_tool", rospy.Time(0))
#Scale by Kp
k, theta = rox.R2rot(error_transform.R)
r = np.multiply(k * theta, Kp[0:3])
r_norm = np.linalg.norm(r)
if (r_norm < 1e-6):
error_transform2_R = np.eye(3)
else:
error_transform2_R = rox.rot(r / r_norm, r_norm)
error_transform2_p = np.multiply(error_transform.p, (Kp[3:6]))
error_transform2 = rox.Transform(error_transform2_R, error_transform2_p)
gripper_to_fixed_marker_tf = gripper_to_camera_tf * fixed_marker_transform
gripper_to_desired_fixed_marker_tf = gripper_to_fixed_marker_tf * error_transform2
#print gripper_to_fixed_marker_tf
ret = world_to_vacuum_gripper_tool_tf * (
gripper_to_desired_fixed_marker_tf *
gripper_to_fixed_marker_tf.inv()).inv()
#print world_to_vacuum_gripper_tool_tf
#print ret
print error_transform
return ret
def puma260b_robot_tool():
robot = puma260b_robot()
robot.R_tool = rox.rot([0, 1, 0], np.pi / 2.0)
robot.p_tool = [0.05, 0, 0]
return robot
def _rot_test(self, k, theta, rot_t):
rot = rox.rot(k, theta)
np.testing.assert_allclose(rot, rot_t, atol=1e-5)
def runTest(self):
x = [1, 0, 0]
y = [0, 1, 0]
z = [0, 0, 1]
#subproblem0
assert (rox.subproblem0(x, y, z) == np.pi / 2)
#subproblem1
k1 = (np.add(x, z)) / np.linalg.norm(np.add(x, z))
k2 = (np.add(y, z)) / np.linalg.norm(np.add(y, z))
assert (rox.subproblem1(k1, k2, z) == np.pi / 2)
#subproblem2
p2 = x
q2 = np.add(x, np.add(y, z))
q2 = q2 / np.linalg.norm(q2)
a2 = rox.subproblem2(p2, q2, z, y)
assert len(a2) == 2
r1 = rox.rot(z, a2[0][0]).dot(rox.rot(y, a2[0][1]))[:, 0]
r2 = rox.rot(z, a2[1][0]).dot(rox.rot(y, a2[1][1]))[:, 0]
np.testing.assert_allclose(r1, q2, atol=1e-4)
np.testing.assert_allclose(r2, q2, atol=1e-4)
a3 = rox.subproblem2(x, z, z, y)
assert len(a3) == 1
r3 = rox.rot(z, a3[0][0]).dot(rox.rot(y, a3[0][1]))[:, 0]
np.testing.assert_allclose(r3, z, atol=1e-4)
#subproblem3
p4 = [.5, 0, 0]
q4 = [0, .75, 0]
a4 = rox.subproblem3(p4, q4, z, .5)
a5 = rox.subproblem3(p4, q4, z, 1.25)
assert len(a4) == 2
np.testing.assert_allclose(
np.linalg.norm(np.add(q4,
rox.rot(z, a4[0]).dot(p4))), 0.5)
np.testing.assert_allclose(
np.linalg.norm(np.add(q4,
rox.rot(z, a4[1]).dot(p4))), 0.5)
assert len(a5) == 1
np.testing.assert_allclose(
np.linalg.norm(np.add(q4,
rox.rot(z, a5[0]).dot(p4))), 1.25)
#subproblem4
p6 = y
q6 = [.8, .2, .5]
d6 = .3
a6 = rox.subproblem4(p6, q6, z, d6)
np.testing.assert_allclose(np.dot(p6,
rox.rot(z, a6[0]).dot(q6)),
d6,
atol=1e-4)
np.testing.assert_allclose(np.dot(p6,
rox.rot(z, a6[1]).dot(q6)),
d6,
atol=1e-4)
def joystick_state_cb(self, joy_state):
with self._lock:
group = self.jog_joints_joystick_group
frame = self.jog_cartesian_joystick_frame
if group < 0 and frame is None:
return
# Rate limit command sends
now = time.time()
have_command = False
if now - self.jog_joints_joystick_last_enable_time > 0.2:
self.jog_joints_joystick_group = -1
else:
have_command = True
if now - self.jog_cartesian_joystick_last_enable_time > 0.2:
self.jog_cartesian_joystick_frame = None
else:
have_command = True
if not have_command:
return
if now - self.joystick_last_command_time < 0.05:
return
self.joystick_last_command_time = now
try:
joy_vals = joy_state.axes / 32767.0
for i in range(len(joy_vals)):
if joy_vals[i] > 0:
if joy_vals[i] < self.joystick_deadzone:
joy_vals[i] = 0
else:
joy_vals[i] = (joy_vals[i] - self.joystick_deadzone
) * (1 - self.joystick_deadzone)
if joy_vals[i] < 0:
if -joy_vals[i] < self.joystick_deadzone:
joy_vals[i] = 0
else:
joy_vals[i] = (joy_vals[i] + self.joystick_deadzone
) * (1 - self.joystick_deadzone)
if group >= 0:
jog_command = np.zeros((self.num_joints, ), dtype=np.float64)
if self.num_joints == 6:
if group == 0:
jog_command[0:3] = joy_vals[0:3]
else:
jog_command[3:6] = joy_vals[0:3]
elif self.num_joints == 7:
if group == 0:
jog_command[0:4] = joy_vals[0:4]
else:
jog_command[4:7] = joy_vals[0:3]
else:
return
jog_command = 0.01 * self.jog_joints_joystick_speed_perc * np.multiply(
jog_command, self.joint_vel_limits) * 0.25
self.jog_joints_with_limits2(jog_command, 0.2, False)
elif frame is not None:
if frame == "robot":
R_axis = joy_vals[3:6] * np.deg2rad(45)
P_axis = joy_vals[0:3] * 0.254
R_spacemouse = rox.rot([1, 0, 0], np.pi)
R_axis = R_spacemouse @ R_axis
P_axis = R_spacemouse @ P_axis
#TODO: Rotate frame about X 180 degrees
qdot = self.update_qdot2(
R_axis, P_axis, self.jog_cartesian_joystick_speed_perc)
self.robot.jog_joint(qdot, 0.2, False)
except:
traceback.print_exc()
def _do_grab_place_object_planar(self, robot_local_device_name,
tool_local_device_name,
robot_origin_calib_global_name,
reference_pose_global_name, object_x,
object_y, object_theta, z_offset_before,
z_offset_grab, speed_perc, grab):
var_storage = self.device_manager.get_device_client(
"variable_storage", 0.1)
robot = self.device_manager.get_device_client(robot_local_device_name)
tool = self.device_manager.get_device_client(tool_local_device_name)
reference_pose = var_storage.getf_variable_value(
"globals", reference_pose_global_name)
robot_origin_calib = var_storage.getf_variable_value(
"globals", robot_origin_calib_global_name)
T_robot = self._geom_util.named_pose_to_rox_transform(
robot_origin_calib.data.pose)
robot_info = robot.robot_info
rox_robot = self._robot_util.robot_info_to_rox_robot(robot_info, 0)
# The object's pose in world coordinates
T_object = rox.Transform(rox.rot([0., 0., 1.], object_theta),
[object_x, object_y, 0.])
# The robot's reference pose in its own frame
T_reference_pose_r = rox.fwdkin(rox_robot,
np.deg2rad(reference_pose.data))
# The robot's reference pose in world coordinates
T_reference_pose = T_robot * T_reference_pose_r
# The nominal grab pose copy
T_grab_pose_n = copy.deepcopy(T_reference_pose)
# Translate the reference pose in x and y to the nominal grab point
T_grab_pose_n.p[0] = T_object.p[0]
T_grab_pose_n.p[1] = T_object.p[1]
# Rotate the reference pose to align with the object
T_grab_pose_n.R = T_object.R @ T_grab_pose_n.R
# Before pose
T_grab_pose_before = copy.deepcopy(T_grab_pose_n)
T_grab_pose_before.p[2] += z_offset_before
# Transform to robot frame
T_grab_pose_before_r = T_robot.inv() * T_grab_pose_before
# Grab pose
T_grab_pose = copy.deepcopy(T_grab_pose_n)
T_grab_pose.p[2] += z_offset_grab
# Transform to robot frame
T_grab_pose_r = T_robot.inv() * T_grab_pose
robot_state, _ = robot.robot_state.PeekInValue()
q_current = robot_state.joint_position
## Compute inverse kinematics
# q_grab_before, res = invkin.update_ik_info3(rox_robot, T_grab_pose_before_r, q_current)
# assert res, "Invalid setpoint: invkin did not converge"
# q_grab, res = invkin.update_ik_info3(rox_robot, T_grab_pose_r, q_current)
# assert res, "Invalid setpoint: invkin did not converge"
# print(f"q_grab_before: {q_grab_before}")
# print(f"q_grab: {q_grab}")
# print()
final_seed = np.deg2rad(reference_pose.data)
traj_before = self._generate_movel_trajectory_tool_j_range(
robot, rox_robot, q_current, T_grab_pose_before_r, speed_perc,
final_seed)
q_init_grab = traj_before.waypoints[-1].joint_position
traj_grab = self._generate_movel_trajectory_tool_j_range(
robot, rox_robot, q_init_grab, T_grab_pose_r, speed_perc,
q_init_grab)
q_init_after = traj_grab.waypoints[-1].joint_position
traj_after = self._generate_movel_trajectory_tool_j_range(
robot, rox_robot, q_init_after, T_grab_pose_before_r, speed_perc,
q_init_grab)
gen = PickPlaceMotionGenerator(robot, rox_robot, tool, traj_before,
traj_grab, traj_after, grab, self._node)
# robot.jog_freespace(q_grab_before,max_velocity,True)
# time.sleep(0.5)
# robot.jog_freespace(q_grab,max_velocity*.25,True)
# time.sleep(0.5)
# robot.jog_freespace(q_grab_before,max_velocity*.25,True)
return gen
def _do_aruco_detection(self, img, intrinsic_global_name,
extrinsic_global_name, aruco_dict_str, aruco_id,
aruco_markersize, roi):
intrinsic_calib = None
extrinsic_calib = None
if intrinsic_global_name is not None and extrinsic_global_name is not None:
var_storage = self.device_manager.get_device_client(
"variable_storage", 0.1)
intrinsic_calib = var_storage.getf_variable_value(
"globals", intrinsic_global_name).data
extrinsic_calib = var_storage.getf_variable_value(
"globals", extrinsic_global_name).data
display_img = img.copy()
assert aruco_dict_str.startswith(
"DICT_"), "Invalid aruco dictionary name"
aruco_dict_i = getattr(aruco,
aruco_dict_str) # convert string to python
aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_i)
aruco_params = cv2.aruco.DetectorParameters_create()
aruco_params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
corners1, ids1, rejected = cv2.aruco.detectMarkers(
img, aruco_dict, parameters=aruco_params)
if corners1 is None:
corners1 = []
if ids1 is None:
ids1 = []
if aruco_id < 0:
corners2 = corners1
ids2 = ids1
else:
corners2 = []
ids2 = []
for id2, corner2 in zip(ids1, corners1):
if id2 == aruco_id:
corners2.append(corner2)
ids2.append(id2)
ids2 = np.array(ids2)
if roi is None:
corners = corners2
ids = ids2
else:
roi_x = roi.center.pose[0]["position"]["x"]
roi_y = roi.center.pose[0]["position"]["y"]
roi_theta = roi.center.pose[0]["orientation"]
roi_w = roi.size[0]["width"]
roi_h = roi.size[0]["height"]
geom_roi1 = shapely.geometry.box(-roi_w / 2.,
-roi_h / 2.,
roi_w / 2.,
roi_h / 2.,
ccw=True)
geom_roi2 = shapely.affinity.translate(geom_roi1,
xoff=roi_x,
yoff=roi_y)
geom_roi = shapely.affinity.rotate(geom_roi2,
roi_theta,
origin='centroid',
use_radians=True)
corners = []
ids = []
for id3, corner3 in zip(ids2, corners2):
centroid = shapely.geometry.Polygon([
shapely.geometry.Point(corner3[0, i, 0], corner3[0, i, 1])
for i in range(4)
])
if geom_roi.contains(centroid):
corners.append(corner3)
ids.append(id3)
ids = np.array(ids)
roi_outline = np.array([geom_roi.exterior.coords], dtype=np.int32)
display_img = cv2.polylines(display_img,
roi_outline,
True,
color=(255, 255, 0))
if len(ids) > 0:
display_img = aruco.drawDetectedMarkers(display_img, corners, ids)
poses = None
if intrinsic_calib is not None and extrinsic_calib is not None:
poses = []
mtx = intrinsic_calib.K
d = intrinsic_calib.distortion_info.data
dist = np.array([d.k1, d.k2, d.p1, d.p2, d.k3])
T_cam = self._geom_util.named_pose_to_rox_transform(
extrinsic_calib.pose)
for id4, corner4 in zip(ids, corners):
rvec, tvec, markerPoints = cv2.aruco.estimatePoseSingleMarkers(
corner4, aruco_markersize, mtx, dist)
display_img = cv2.aruco.drawAxis(display_img, mtx, dist, rvec,
tvec, 0.05)
# R_marker1 = cv2.Rodrigues(rvec.flatten())[0]
# TODO: 3D pose estimation from rvec is very innaccurate. Use a simple trigonometry
# to estimate the Z rotation of the tag
# compute vectors from opposite corners
v1 = corner4[0, 2, :] - corner4[0, 0, :]
v2 = corner4[0, 3, :] - corner4[0, 1, :]
# Use atan2 on each vector and average
theta1 = (np.arctan2(v1[1], v1[0]) - np.deg2rad(45)) % (2. *
np.pi)
theta2 = (np.arctan2(v2[1], v2[0]) - np.deg2rad(135)) % (2. *
np.pi)
theta = (theta1 + theta2) / 2.
R_marker1 = rox.rot([1., 0., 0.], np.pi) @ rox.rot(
[0., 0., -1.], theta)
p_marker1 = tvec.flatten()
T_marker1 = rox.Transform(R_marker1, p_marker1, "camera",
f"aruco{int(id4)}")
T_marker = T_cam * T_marker1
rr_marker_pose = self._geom_util.rox_transform_to_named_pose(
T_marker)
poses.append(rr_marker_pose)
ret_markers = []
if poses is None:
poses = [None] * len(ids)
for id_, corner, pose in zip(ids, corners, poses):
m = self._detected_marker()
m.marker_id = int(id_)
m.marker_corners = np.zeros((4, ), dtype=self._point2d_dtype)
for i in range(4):
m.marker_corners[i] = self._geom_util.xy_to_point2d(
corner[0, i, :])
m.marker_pose = pose
ret_markers.append(m)
ret = self._aruco_detection_result()
ret.detected_markers = ret_markers
ret.display_image = self._image_util.array_to_compressed_image_jpg(
display_img, 70)
return ret
def _rpy_to_rot(rpy):
return rox.rot([0,0,1],rpy[2]).dot(rox.rot([0,1,0],rpy[1]))\
.dot(rox.rot([1,0,0],rpy[0]))
def robot6_sphericalwrist_invkin(robot, desired_pose, last_joints=None):
"""
Inverse kinematics for six axis articulated industrial robots
with sherical wrists. Examples include Puma 260, ABB IRB6640,
Staubli TX40, etc. Note that this is not for Universal Robot
wrist configurations.
:type robot: general_robotics_toolbox.Robot
:param robot: The robot object representing the geometry of the robot
:type desired_pose: general_robotics_toolbox.Transform
:param desired_pose: The desired pose of the robot
:type last_joints: list, tuple, or numpy.array
:param last_joints: The joints of the robot at the last timestep. The returned
first returned joint configuration will be the closests to last_joints. Optional
:rtype: list of numpy.array
:return: A list of zero or more joint angles that match the desired pose. An
empty list means that the desired pose cannot be reached. If last_joints
is specified, the first entry is the closest configuration to last_joints.
"""
R06 = desired_pose.R
p0T = desired_pose.p
if robot.R_tool is not None and robot.p_tool is not None:
R06 = R06.dot(np.transpose(robot.R_tool))
p0T = p0T - R06.dot(robot.p_tool)
H = robot.H
P = robot.P
theta_v = []
#Correct for spherical joint position vectors
if not np.all(P[:, 4] == 0):
P4_d = P[:, 4].dot(H[:, 3])
assert np.all(P[:, 4] - P4_d * H[:, 3] == 0)
P[:, 3] += P[:, 4]
P[:, 4] = np.zeros(3)
if not np.all(P[:, 5] == 0):
P5_d = P[:, 5].dot(H[:, 5])
# assert np.all(P[:,5] - P5_d*H[:,5] == 0)
P[:, 6] += P[:, 5]
P[:, 5] = np.zeros(3)
d1 = np.dot(ey, P[:, 1] + P[:, 2] + P[:, 3])
v1 = p0T - R06.dot(P[:, 6])
p1 = ey
Q1 = rox.subproblem4(p1, v1, -H[:, 0], d1)
normalize = normalize_joints(robot, last_joints)
for q1 in normalize(0, Q1):
R01 = rox.rot(H[:, 0], q1)
p26_q1 = R01.T.dot(p0T - R06.dot(P[:, 6])) - (P[:, 0] + P[:, 1])
d3 = np.linalg.norm(p26_q1)
v3 = P[:, 2]
p3 = P[:, 3]
Q3 = rox.subproblem3(p3, v3, H[:, 2], d3)
for q3 in normalize(2, Q3):
R23 = rox.rot(H[:, 2], q3)
v2 = p26_q1
p2 = P[:, 2] + R23.dot(P[:, 3])
q2 = rox.subproblem1(p2, v2, H[:, 1])
q2 = normalize(1, [q2])
if len(q2) == 0:
continue
q2 = q2[0]
R12 = rox.rot(H[:, 1], q2)
R03 = R01.dot(R12).dot(R23)
R36 = R03.T.dot(R06)
v4 = R36.dot(H[:, 5])
p4 = H[:, 5]
Q4_Q5 = rox.subproblem2(p4, v4, H[:, 3], H[:, 4])
for q4, q5 in normalize((3, 4), Q4_Q5):
R35 = rox.rot(H[:, 3], q4).dot(rox.rot(H[:, 4], q5))
R05 = R03.dot(R35)
R56 = R05.T.dot(R06)
p6 = H[:, 4]
v6 = R56.dot(H[:, 4])
q6 = rox.subproblem1(p6, v6, H[:, 5])
q6 = normalize(5, [q6])
if len(q6) == 0:
continue
q6 = q6[0]
theta_v.append(np.array([q1, q2, q3, q4, q5, q6]))
if last_joints is not None:
theta_dist = np.linalg.norm(np.subtract(theta_v, last_joints), axis=1)
return [theta_v[i] for i in list(np.argsort(theta_dist))]
else:
return theta_v
def _generate_movel_trajectory(self, robot_client, rox_robot,
initial_q_or_T, T_final, speed_perc,
q_final_seed):
JointTrajectoryWaypoint = RRN.GetStructureType(
"com.robotraconteur.robotics.trajectory.JointTrajectoryWaypoint",
robot_client)
JointTrajectory = RRN.GetStructureType(
"com.robotraconteur.robotics.trajectory.JointTrajectory",
robot_client)
dof = len(rox_robot.joint_type)
if isinstance(initial_q_or_T, rox.Robot):
T_initial = initial_q_or_T
q_initial = invkin.update_ik_info3(rox_robot, initial_q_or_T,
np.random.randn((dof, )))
else:
q_initial = initial_q_or_T
T_initial = rox.fwdkin(rox_robot, q_initial)
p_diff = T_final.p - T_initial.p
R_diff = np.transpose(T_initial.R) @ T_final.R
k, theta = rox.R2rot(R_diff)
N_samples_translate = np.linalg.norm(p_diff) * 100.0
N_samples_rot = np.abs(theta) * 0.25 * 180 / np.pi
N_samples_translate = np.linalg.norm(p_diff) * 100.0
N_samples_rot = np.abs(theta) * 0.2 * 180 / np.pi
N_samples = math.ceil(np.max((N_samples_translate, N_samples_rot, 20)))
ss = np.linspace(0, 1, N_samples)
if q_final_seed is None:
q_final, res = invkin.update_ik_info3(rox_robot, T_final,
q_initial)
else:
q_final, res = invkin.update_ik_info3(rox_robot, T_final,
q_final_seed)
#assert res, "Inverse kinematics failed"
way_pts = np.zeros((N_samples, dof), dtype=np.float64)
way_pts[0, :] = q_initial
for i in range(1, N_samples):
s = float(i) / (N_samples - 1)
R_i = T_initial.R @ rox.rot(k, theta * s)
p_i = (p_diff * s) + T_initial.p
T_i = rox.Transform(R_i, p_i)
#try:
# q, res = invkin.update_ik_info3(rox_robot, T_i, way_pts[i-1,:])
#except AssertionError:
ik_seed = (1.0 - s) * q_initial + s * q_final
q, res = invkin.update_ik_info3(rox_robot, T_i, ik_seed)
assert res, "Inverse kinematics failed"
way_pts[i, :] = q
vlims = rox_robot.joint_vel_limit
alims = rox_robot.joint_acc_limit
path = ta.SplineInterpolator(ss, way_pts)
pc_vel = constraint.JointVelocityConstraint(vlims * 0.95 * speed_perc /
100.0)
pc_acc = constraint.JointAccelerationConstraint(alims)
instance = algo.TOPPRA([pc_vel, pc_acc],
path,
parametrizer="ParametrizeConstAccel")
jnt_traj = instance.compute_trajectory()
if jnt_traj is None:
return None
ts_sample = np.linspace(0, jnt_traj.duration, N_samples)
qs_sample = jnt_traj(ts_sample)
waypoints = []
for i in range(N_samples):
wp = JointTrajectoryWaypoint()
wp.joint_position = qs_sample[i, :]
wp.time_from_start = ts_sample[i]
waypoints.append(wp)
traj = JointTrajectory()
traj.joint_names = rox_robot.joint_names
traj.waypoints = waypoints
return traj
