def CalcPoseError(self, X_WE_desired, q):
pose = self.ForwardKinematics(q)
err_vec = np.zeros(6)
err_vec[-3:] = X_WE_desired.translation() - pose.translation()
rot_err = AngleAxis(X_WE_desired.rotation() *
pose.rotation().transpose())
err_vec[:3] = rot_err.axis() * rot_err.angle()
def _revolute_feedback(self, joint, feedback):
if feedback.event_type != InteractiveMarkerFeedback.POSE_UPDATE:
return
expected_frame = joint.child_body().body_frame().name()
if expected_frame != feedback.header.frame_id:
# TODO(sloretz) fix tf2_geometry_msgs_py :(
# transformed_point = self._tf_buffer.transform(point_stamped, self._frame_id)
print("TODO accept feedback in different frame")
return
qw = feedback.pose.orientation.w
qx = feedback.pose.orientation.x
qy = feedback.pose.orientation.y
qz = feedback.pose.orientation.z
new_orientation = Quaternion(qw, qx, qy, qz)
prev_orientation = self._joint_prev_orientation[joint.name()]
orientation_diff = prev_orientation.inverse().multiply(new_orientation)
diff_aa = AngleAxis(orientation_diff)
joint_axis = self._joint_axis_in_child[joint.name()]
dot = numpy.dot(joint_axis, diff_aa.axis())
if dot > 0.999:
angle_inc = diff_aa.angle()
elif dot < -0.999:
angle_inc = -1 * diff_aa.angle()
else:
angle_inc = 0.
angle = self._joint_states[self._joint_indexes[joint.name()]] + angle_inc
if angle > joint.position_upper_limit():
angle = joint.position_upper_limit()
elif angle < joint.position_lower_limit():
angle = joint.position_lower_limit()
self._joint_states[self._joint_indexes[joint.name()]] = angle
self._joint_prev_orientation[joint.name()] = new_orientation
def compare(self, b, cmp_, parent_path):
"""Compares to another frame `b` given `cmp_` metrics."""
path = parent_path + "/frame[@name='{}']".format(self.name)
assert self.name == b.name, path
X_AB = self.X_WF.inverse().multiply(b.X_WF)
rot_err = AngleAxis(X_AB.rotation()).angle()
if np.abs(rot_err - np.pi) < np.abs(rot_err):
rot_err -= np.pi
tr_err = np.linalg.norm(X_AB.translation())
if np.abs(tr_err) > cmp_.tol or np.abs(rot_err) > cmp_.tol:
return not cmp_.error(
KinematicError(path, self, b, rot_err, tr_err))
return True
def test_rigid_transform(self):
def check_equality(X_actual, X_expected_matrix):
# TODO(eric.cousineau): Use `IsNearlyEqualTo`.
self.assertIsInstance(X_actual, mut.RigidTransform)
self.assertTrue(
np.allclose(X_actual.GetAsMatrix4(), X_expected_matrix))
# - Constructors.
X_I = np.eye(4)
check_equality(mut.RigidTransform(), X_I)
check_equality(mut.RigidTransform(other=mut.RigidTransform()), X_I)
check_equality(copy.copy(mut.RigidTransform()), X_I)
R_I = mut.RotationMatrix()
p_I = np.zeros(3)
rpy_I = mut.RollPitchYaw(0, 0, 0)
quaternion_I = Quaternion.Identity()
angle = np.pi * 0
axis = [0, 0, 1]
angle_axis = AngleAxis(angle=angle, axis=axis)
check_equality(mut.RigidTransform(R=R_I, p=p_I), X_I)
check_equality(mut.RigidTransform(rpy=rpy_I, p=p_I), X_I)
check_equality(mut.RigidTransform(quaternion=quaternion_I, p=p_I), X_I)
check_equality(mut.RigidTransform(theta_lambda=angle_axis, p=p_I), X_I)
check_equality(mut.RigidTransform(R=R_I), X_I)
check_equality(mut.RigidTransform(p=p_I), X_I)
# - Accessors, mutators, and general methods.
X = mut.RigidTransform()
X.set(R=R_I, p=p_I)
X.SetFromIsometry3(pose=Isometry3.Identity())
check_equality(mut.RigidTransform.Identity(), X_I)
self.assertIsInstance(X.rotation(), mut.RotationMatrix)
X.set_rotation(R=R_I)
self.assertIsInstance(X.translation(), np.ndarray)
X.set_translation(p=np.zeros(3))
self.assertTrue(np.allclose(X.GetAsMatrix4(), X_I))
self.assertTrue(np.allclose(X.GetAsMatrix34(), X_I[:3]))
self.assertIsInstance(X.GetAsIsometry3(), Isometry3)
check_equality(X.inverse(), X_I)
self.assertIsInstance(
X.multiply(other=mut.RigidTransform()), mut.RigidTransform)
self.assertIsInstance(X.multiply(p_BoQ_B=p_I), np.ndarray)
if six.PY3:
self.assertIsInstance(
eval("X @ mut.RigidTransform()"), mut.RigidTransform)
self.assertIsInstance(eval("X @ [0, 0, 0]"), np.ndarray)
def test_quaternion_slerp(self):
# Test empty constructor.
pq = PiecewiseQuaternionSlerp()
self.assertEqual(pq.rows(), 4)
self.assertEqual(pq.cols(), 1)
self.assertEqual(pq.get_number_of_segments(), 0)
t = [0., 1., 2.]
# Make identity rotations.
q = Quaternion()
m = np.identity(3)
a = AngleAxis()
R = RotationMatrix()
# Test quaternion constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, quaternions=[q, q, q])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[m, m, m])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test axis angle constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, angle_axes=[a, a, a])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test rotation matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[R, R, R])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test append operations.
pq.Append(time=3., quaternion=q)
pq.Append(time=4., rotation_matrix=R)
pq.Append(time=5., angle_axis=a)
def test_quaternion_slerp(self):
# Test empty constructor.
pq = PiecewiseQuaternionSlerp()
self.assertEqual(pq.rows(), 4)
self.assertEqual(pq.cols(), 1)
self.assertEqual(pq.get_number_of_segments(), 0)
t = [0., 1., 2.]
# Make identity rotations.
q = Quaternion()
m = np.identity(3)
a = AngleAxis()
R = RotationMatrix()
# Test quaternion constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, quaternions=[q, q, q])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[m, m, m])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test axis angle constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, angle_axes=[a, a, a])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test rotation matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[R, R, R])
self.assertEqual(pq.get_number_of_segments(), 2)
np.testing.assert_equal(pq.value(0.5), np.eye(3))
# Test append operations.
pq.Append(time=3., quaternion=q)
pq.Append(time=4., rotation_matrix=R)
pq.Append(time=5., angle_axis=a)
# Test getters.
pq = PiecewiseQuaternionSlerp(
breaks=[0, 1], angle_axes=[a, AngleAxis(np.pi / 2, [0, 0, 1])])
np.testing.assert_equal(np.array([1, 0, 0, 0]),
pq.orientation(time=0).wxyz())
np.testing.assert_allclose(
np.array([0, 0, np.pi / 2]),
pq.angular_velocity(time=0),
atol=1e-15,
rtol=0,
)
np.testing.assert_equal(np.zeros(3), pq.angular_acceleration(time=0))
np.testing.assert_allclose(
np.array([np.cos(np.pi / 4), 0, 0,
np.sin(np.pi / 4)]),
pq.orientation(time=1).wxyz(),
atol=1e-15,
rtol=0,
)
np.testing.assert_allclose(
np.array([0, 0, np.pi / 2]),
pq.angular_velocity(time=1),
atol=1e-15,
rtol=0,
)
np.testing.assert_equal(np.zeros(3), pq.angular_acceleration(time=1))
temp_plant_context = plant.GetMyContextFromRoot(temp_context)
desired_initial_state = np.array([0.0, 0.3, 0.0, -1.3, 0.0, 1.65, 0.9, 0.040, 0.040])
plant.SetPositions(temp_plant_context, panda, desired_initial_state)
X_G = {"initial": plant.EvalBodyPoseInWorld(temp_plant_context, plant.GetBodyByName("panda_hand"))}
# X_G = {"initial": RigidTransform(RotationMatrix.MakeXRotation(np.pi), [0, -0.25, 0.25])}
X_G["pick"] = X_O["initial"].multiply(X_OGgrasp)
X_G["prepick"] = X_G["pick"].multiply(X_GgraspGpregrasp)
X_G["place"] = X_O["goal"].multiply(X_OGgrasp)
X_G["preplace"] = X_G["place"].multiply(X_GgraspGpregrasp)
# Interpolate a halfway orientation by converting to axis angle and halving angle
X_GprepickGpreplace = X_G["prepick"].inverse().multiply(X_G["preplace"])
angle_axis = X_GprepickGpreplace.rotation().ToAngleAxis()
X_GprepickGclearance = RigidTransform(AngleAxis(angle=angle_axis.angle() / 2.0, axis=angle_axis.axis()),
X_GprepickGpreplace.translation() / 2.0 + np.array([0, 0.0, -0.3]))
X_G["clearance"] = X_G["prepick"].multiply(X_GprepickGclearance)
# Precise timings of trajectory
times = {"initial": 0}
X_GinitialGprepick = X_G["initial"].inverse().multiply(X_G["prepick"])
times["prepick"] = times["initial"] + 10.0 * np.linalg.norm(X_GinitialGprepick.translation())
# Allow some time for gripper to close
times["pick_start"] = times["prepick"] + 2.0
times["pick_end"] = times["pick_start"] + 2.0
times["postpick"] = times["pick_end"] + 2.0
time_to_from_clearance = 10.0 * np.linalg.norm(X_GprepickGclearance.translation())
times["clearance"] = times["postpick"] + time_to_from_clearance
times["preplace"] = times["clearance"] + time_to_from_clearance
def rotation_matrix_to_axang3(R):
assert isinstance(R, RotationMatrix), repr(type(R))
axang = AngleAxis(R.matrix())
axang3 = axang.angle() * axang.axis()
return axang3
def DoCalcAbstractOutput(self, context, y_data):
self.callback_lock.acquire()
if self.selected_body and self.grab_in_progress:
# Simple inverse dynamics PD rule to drive object to desired pose.
body = self.selected_body
# Load in robot state
x_in = self.EvalVectorInput(context, 1).get_value()
self.mbp.SetPositionsAndVelocities(self.mbp_context, x_in)
TF_object = self.mbp.EvalBodyPoseInWorld(self.mbp_context, body)
xyz = TF_object.translation()
R = TF_object.rotation().matrix()
TFd_object = self.mbp.EvalBodySpatialVelocityInWorld(
self.mbp_context, body)
xyzd = TFd_object.translational()
Rd = TFd_object.rotational()
# Match the object position directly to the hydra position.
xyz_desired = self.desired_pose_in_world_frame.translation()
# Regress xyz back to just the hydra pose in the attraction case
if self.previously_freezing_rotation != self.freeze_rotation:
self.selected_body_init_offset = TF_object
self.grab_update_hydra_pose = RigidTransform(
self.desired_pose_in_world_frame)
self.previously_freezing_rotation = self.freeze_rotation
if self.freeze_rotation:
R_desired = self.selected_body_init_offset.rotation().matrix()
else:
# Figure out the relative rotation of the hydra from its initial posture
to_init_hydra_tf = self.grab_update_hydra_pose.inverse()
desired_delta_rotation = to_init_hydra_tf.multiply(
self.desired_pose_in_world_frame).matrix()[:3, :3]
# Transform the current object rotation into the init hydra frame, apply that relative tf, and
# then transform back
to_init_hydra_tf_rot = to_init_hydra_tf.matrix()[:3, :3]
R_desired = to_init_hydra_tf_rot.T.dot(
desired_delta_rotation.dot(
to_init_hydra_tf_rot.dot(
self.selected_body_init_offset.rotation().matrix())
))
# Could also pull the rotation back, but it's kind of nice to be able to recenter the object
# without messing up a randomized rotation.
#R_desired = (self.desired_pose_in_world_frame.rotation().matrix()*self.attract_factor +
# R_desired*(1.-self.attract_factor))
# Apply PD in cartesian space
xyz_e = xyz_desired - xyz
xyzd_e = -xyzd
f = 100. * xyz_e + 10. * xyzd_e
R_err_in_body_frame = np.linalg.inv(R).dot(R_desired)
aa = AngleAxis(R_err_in_body_frame)
tau_p = R.dot(aa.axis() * aa.angle())
tau_d = -Rd
tau = tau_p + 0.1 * tau_d
exerted_force = SpatialForce(tau=tau, f=f)
out = ExternallyAppliedSpatialForce()
out.F_Bq_W = exerted_force
out.body_index = self.selected_body.index()
y_data.set_value(VectorExternallyAppliedSpatialForced([out]))
else:
y_data.set_value(VectorExternallyAppliedSpatialForced([]))
self.callback_lock.release()
def corrupt_frame(frame):
X_err = Isometry3(rotation=AngleAxis(np.pi / 2, [1, 0, 0]).rotation(),
translation=[0, 0, 0])
frame.X_WF = frame.X_WF.multiply(X_err)