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 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()