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 _compute_joint_command(self, controller_state=None):
if controller_state is None:
controller_state = self._state
controller_state.controller_time = self._clock.now
step_ts = controller_state.ts * controller_state.speed_scalar
if controller_state.mode.value < 0:
if controller_state.mode == ControllerMode.ROBOT_INVALID_STATE \
or controller_state.mode == ControllerMode.ROBOT_COMMUNICATION_ERROR \
or controller_state.mode == ControllerMode.ROBOT_FAULT:
controller_state.joint_setpoint = None
controller_state.joint_command = None
controller_state.ft_wrench = None
while True:
self._update_active_trajectory(controller_state)
if controller_state.active_trajectory is not None:
controller_state.active_trajectory.abort(
controller_state.mode)
controller_state.active_trajectory = None
else:
break
return controller_state.mode
else:
self._update_active_trajectory(controller_state)
if controller_state.joint_setpoint is None:
controller_state.joint_setpoint = controller_state.joint_position
if controller_state.ft_wrench_threshold is not None \
and np.shape(controller_state.ft_wrench_threshold) == (6,) \
and controller_state.ft_wrench is None:
controller_state.mode = ControllerMode.SENSOR_FAULT
return ControllerMode.SENSOR_FAULT
if controller_state.joystick_command is not None \
and controller_state.joystick_command.halt_command:
controller_state.mode = ControllerMode.HALT
if controller_state.mode.value > 0 and not self._check_ft_threshold(
controller_state.ft_wrench, controller_state.ft_wrench_bias):
if controller_state.active_trajectory is not None:
controller_state.active_trajectory.abort(ControllerMode.FT_THRESHOLD_VIOLATION, \
"Force/Torque Threshold Violated")
controller_state.active_trajectory = None
controller_state.mode = ControllerMode.FT_THRESHOLD_VIOLATION
return ControllerMode.FT_THRESHOLD_VIOLATION
try:
if controller_state.mode == ControllerMode.HALT:
pass
elif controller_state.mode.value < ControllerMode.HALT.value:
if controller_state.active_trajectory is not None:
controller_state.active_trajectory.abort(
controller_state.mode)
controller_state.active_trajectory = None
elif controller_state.mode == ControllerMode.JOINT_TELEOP:
if controller_state.joystick_command is not None \
and controller_state.joystick_command.joint_velocity_command is not None:
#controller_state.joint_setpoint += \
#controller_state.joystick_command.joint_velocity_command.dot(step_ts)
####################################################################
J = rox.robotjacobian(self._robot,
controller_state.joint_position)
cmd_vel = np.dot(
J, controller_state.joystick_command.
joint_velocity_command)
controller_state.joint_setpoint += self._quadprog.compute_quadprog(
controller_state.joint_position).dot(step_ts * 5)
####################################################################
#self._clip_joint_angles(controller_state)
elif controller_state.mode == ControllerMode.CARTESIAN_TELEOP:
if controller_state.joystick_command is not None \
and controller_state.joystick_command.spatial_velocity_command is not None:
#self._compute_joint_vel_from_spatial_vel(controller_state, step_ts, controller_state.joystick_command.spatial_velocity_command)
####################################################
########## change step_ts*5 #################
controller_state.joint_setpoint += self._quadprog.compute_quadprog(
controller_state.joint_position).dot(step_ts * 2)
#self._clip_joint_angles(controller_state)
####################################################
elif controller_state.mode == ControllerMode.CYLINDRICAL_TELEOP:
if controller_state.joystick_command is not None \
and controller_state.joystick_command.spatial_velocity_command is not None:
if (not all(self._robot.H[:, 0] == (0, 0, 1))
or self._robot.joint_type[0] != 0):
controller_state.mode = ControllerMode.INVALID_OPERATION
else:
cmd_vel = controller_state.joystick_command.spatial_velocity_command
transform_0T = rox.fwdkin(
self._robot, controller_state.joint_position)
d = np.linalg.norm(
(transform_0T.p[0], transform_0T.p[1]))
d = np.max((d, 0.05))
theta = np.arctan2(transform_0T.p[1],
transform_0T.p[0])
s_0 = transform_0T.p[1] / d
c_0 = transform_0T.p[0] / d
v_x = -d * s_0 * cmd_vel[3] + c_0 * cmd_vel[4]
v_y = d * c_0 * cmd_vel[3] + s_0 * cmd_vel[4]
v_z = cmd_vel[5]
w_x = cmd_vel[0]
w_y = cmd_vel[1]
w_z = cmd_vel[2] + cmd_vel[3]
cmd_vel2 = np.array([w_x, w_y, w_z, v_x, v_y, v_z])
#self._compute_joint_vel_from_spatial_vel(controller_state, step_ts, cmd_vel2)
########## change step_ts*5 #################
controller_state.joint_setpoint += self._quadprog.compute_quadprog(
controller_state.joint_position).dot(step_ts * 5)
####################################################
elif controller_state.mode == ControllerMode.SPHERICAL_TELEOP:
if controller_state.joystick_command is not None \
and controller_state.joystick_command.spatial_velocity_command is not None:
if (not all(self._robot.H[:, 0] == (0, 0, 1))
or self._robot.joint_type[0] != 0):
controller_state.mode = ControllerMode.INVALID_OPERATION
else:
#TODO: more clever solution that doesn't require trigonometry?
cmd_vel = controller_state.joystick_command.spatial_velocity_command
transform_0T = rox.fwdkin(
self._robot, controller_state.joint_position)
d = np.linalg.norm(transform_0T.p)
d = np.max((d, 0.05))
theta_phi_res = rox.subproblem2(
np.dot([1, 0, 0], d), transform_0T.p, [0, 0, 1],
[0, 1, 0])
if (len(theta_phi_res) == 0):
controller_state.mode = ControllerMode.ROBOT_SINGULARITY
else:
theta_dot = cmd_vel[3]
phi_dot = cmd_vel[4]
d_dot = cmd_vel[5]
if (len(theta_phi_res) == 1):
theta, phi = theta_phi_res[0]
elif (np.abs(theta_phi_res[0][1]) <
np.deg2rad(90)):
theta, phi = theta_phi_res[0]
else:
theta, phi = theta_phi_res[1]
s_theta = np.sin(theta)
c_theta = np.cos(theta)
s_phi = np.sin(phi)
c_phi = np.cos(phi)
v_x = -d * s_phi * c_theta * phi_dot - d * s_theta * c_theta * theta_dot + c_phi * c_theta * d_dot
v_y = -d * s_phi * s_theta * phi_dot + d * c_phi * c_theta * theta_dot + s_theta * c_phi * d_dot
v_z = -d * c_phi * phi_dot - s_phi * d_dot
w_x = cmd_vel[0] - phi_dot * s_theta
w_y = cmd_vel[1] + phi_dot * c_theta
w_z = cmd_vel[2] + theta_dot
cmd_vel2 = np.array([w_x, w_y, w_z, v_x, v_y, v_z])
#self._compute_joint_vel_from_spatial_vel(controller_state, step_ts, cmd_vel2)
########## change step_ts*5 ###################
controller_state.joint_setpoint += self._quadprog.compute_quadprog(
controller_state.joint_position).dot(step_ts *
5)
###############################################
elif controller_state.mode == ControllerMode.SHARED_TRAJECTORY:
if controller_state.joystick_command is not None \
and controller_state.joystick_command.trajectory_velocity_command is not None:
active_trajectory = controller_state.active_trajectory
if active_trajectory is not None and active_trajectory.trajectory_valid:
res, setpoint = active_trajectory.increment_trajectory_time(
step_ts * controller_state.joystick_command.
trajectory_velocity_command, controller_state)
if res == ControllerMode.SUCCESS:
controller_state.joint_setpoint = setpoint
elif controller_state.mode == ControllerMode.AUTO_TRAJECTORY:
active_trajectory = controller_state.active_trajectory
if active_trajectory is not None and active_trajectory.trajectory_valid:
res, setpoint = active_trajectory.increment_trajectory_time(
step_ts, controller_state)
if res == ControllerMode.SUCCESS:
controller_state.joint_setpoint = setpoint
elif controller_state.mode.value >= ControllerMode.EXTERNAL_SETPOINT_0.value \
and controller_state.mode.value <= ControllerMode.EXTERNAL_SETPOINT_7.value:
i = controller_state.mode.value - ControllerMode.EXTERNAL_SETPOINT_0.value
setpoint = controller_state.external_joint_setpoints[i]
if setpoint is not None:
controller_state.joint_setpoint = np.copy(setpoint)
self._clip_joint_angles(controller_state)
else:
self._compute_setpoint_custom_mode(controller_state)
except:
traceback.print_exc()
controller_state.mode = ControllerMode.INTERNAL_ERROR
controller_state.joint_command = controller_state.joint_position
#TODO: add in joint command filter
controller_state.joint_command = controller_state.joint_setpoint
return controller_state.mode
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