def resetGoalPose(self):
# initial pose will be the center
# of self.lfoot_frame_id and self.rfoot_frame_id
lfoot_pose = tf_ext.transformToMatrix(self.tf_listener.lookupTransform(
self.frame_id,
self.lfoot_frame_id,
rospy.Time(0.0)))
rfoot_pose = tf_ext.transformToMatrix(self.tf_listener.lookupTransform(
self.frame_id,
self.rfoot_frame_id,
rospy.Time(0.0)))
# apply offset
lfoot_with_offset = numpy.dot(lfoot_pose, self.lfoot_offset)
rfoot_with_offset = numpy.dot(rfoot_pose, self.rfoot_offset)
(lfoot_pos, lfoot_q) = tf_ext.decomposeMatrix(lfoot_with_offset)
(rfoot_pos, rfoot_q) = tf_ext.decomposeMatrix(rfoot_with_offset)
# compute the center of the two transformations
mid_pos = (lfoot_pos + rfoot_pos) / 2.0
mid_quaternion = quaternion_slerp(lfoot_q, rfoot_q,
0.5)
self.pre_pose.pose.position.x = mid_pos[0]
self.pre_pose.pose.position.y = mid_pos[1]
self.pre_pose.pose.position.z = mid_pos[2]
self.pre_pose.pose.orientation.x = mid_quaternion[0]
self.pre_pose.pose.orientation.y = mid_quaternion[1]
self.pre_pose.pose.orientation.z = mid_quaternion[2]
self.pre_pose.pose.orientation.w = mid_quaternion[3]
def resetGoalPose(self):
# initial pose will be the center
# of self.lfoot_frame_id and self.rfoot_frame_id
lfoot_pose = tf_ext.transformToMatrix(self.tf_listener.lookupTransform(
self.frame_id,
self.lfoot_frame_id,
rospy.Time(0.0)))
rfoot_pose = tf_ext.transformToMatrix(self.tf_listener.lookupTransform(
self.frame_id,
self.rfoot_frame_id,
rospy.Time(0.0)))
# apply offset
lfoot_with_offset = numpy.dot(lfoot_pose, self.lfoot_offset)
rfoot_with_offset = numpy.dot(rfoot_pose, self.rfoot_offset)
(lfoot_pos, lfoot_q) = tf_ext.decomposeMatrix(lfoot_with_offset)
(rfoot_pos, rfoot_q) = tf_ext.decomposeMatrix(rfoot_with_offset)
# compute the center of the two transformations
mid_pos = (lfoot_pos + rfoot_pos) / 2.0
mid_quaternion = quaternion_slerp(lfoot_q, rfoot_q,
0.5)
self.pre_pose.pose.position.x = mid_pos[0]
self.pre_pose.pose.position.y = mid_pos[1]
self.pre_pose.pose.position.z = mid_pos[2]
self.pre_pose.pose.orientation.x = mid_quaternion[0]
self.pre_pose.pose.orientation.y = mid_quaternion[1]
self.pre_pose.pose.orientation.z = mid_quaternion[2]
self.pre_pose.pose.orientation.w = mid_quaternion[3]