def createFootStepInPlace(stepSide):
footstep = FootstepDataRosMessage()
footstep.robot_side = stepSide
if stepSide == LEFT:
foot_frame = LEFT_FOOT_FRAME_NAME
else:
foot_frame = RIGHT_FOOT_FRAME_NAME
footWorld = tfBuffer.lookup_transform('world', foot_frame, rospy.Time())
footstep.orientation = footWorld.transform.rotation
footstep.location = footWorld.transform.translation
return footstep
def create_one_footstep(self, side, offset, angle=None, world=False):
""" Create one footstep. """
footstep = FootstepDataRosMessage()
footstep.robot_side = side
up = offset[2]
intermediate_pt = None
world_intermediate = None
world_intermediate2 = None
# if up > 0.05 and side == FootstepDataRosMessage.RIGHT and self.USE_RIGHT_STEP_UP_TRAJECTORY:
if up > 0.05 and self.USE_STEP_UP_TRAJECTORY:
# Note, how we use 0.06 or 0.03 forward, which would intuitively seem wrong, but it produces the desired
# effect.
xoffset = 0.06 if side == FootstepDataRosMessage.LEFT else 0.03
intermediate_pt = [xoffset, 0, up + 0.05]
if side == FootstepDataRosMessage.LEFT:
footstep_type = "left"
footstep.orientation = deepcopy(self.lf_start_orientation)
footstep.location = deepcopy(self.lf_start_position)
if not world:
world_offset = self.transform_left_offset(offset)
if intermediate_pt is not None:
log("Using waypoints to move left foot to avoid stair")
lf_start_v = msg_v_to_v(self.lf_start_position)
world_intermediate = add_v(
lf_start_v,
self.transform_left_offset(intermediate_pt))
world_intermediate2 = add_v(lf_start_v, world_offset)
else:
world_offset = offset
else:
footstep_type = "right"
footstep.orientation = deepcopy(self.rf_start_orientation)
footstep.location = deepcopy(self.rf_start_position)
if not world:
world_offset = self.transform_right_offset(offset)
if intermediate_pt is not None:
log("Using waypoints to move right foot to avoid stair")
rf_start_v = msg_v_to_v(self.rf_start_position)
world_intermediate = add_v(
rf_start_v,
self.transform_right_offset(intermediate_pt))
world_intermediate2 = add_v(rf_start_v, world_offset)
else:
world_offset = offset
if world_intermediate is None:
footstep.trajectory_type = FootstepDataRosMessage.DEFAULT
else:
footstep.trajectory_type = FootstepDataRosMessage.CUSTOM
footstep.trajectory_waypoints = [
v_to_msg_v(world_intermediate),
v_to_msg_v(world_intermediate2)
]
reference_foot_angle = compute_rot_angle(self.lf_start_orientation)
if not world:
log("Preparing " + footstep_type + " foot (left foot rotated=" +
fmt(reference_foot_angle) + "), moving " + fmt(offset))
else:
log("Preparing " + footstep_type + " foot, move (world) " +
fmt(offset))
footstep.location.x += world_offset[0]
footstep.location.y += world_offset[1]
footstep.location.z += world_offset[2]
if angle is not None:
# Rotate about z-axis.
rotate = quaternion_about_axis(radians(angle), [0, 0, 1])
original = [
footstep.orientation.x, footstep.orientation.y,
footstep.orientation.z, footstep.orientation.w
]
new = quaternion_multiply(rotate, original)
log("Rotating " + footstep_type + " foot by " + fmt(angle))
footstep.orientation = Quaternion(new[0], new[1], new[2], new[3])
return footstep
def createRotationFootStepList(self, yaw):
left_footstep = FootstepDataRosMessage()
left_footstep.robot_side = self.LEFT_FOOT
right_footstep = FootstepDataRosMessage()
right_footstep.robot_side = self.RIGHT_FOOT
left_foot_world = self.tfBuffer.lookup_transform(
'world', self.LEFT_FOOT_FRAME_NAME, rospy.Time())
right_foot_world = self.tfBuffer.lookup_transform(
'world', self.RIGHT_FOOT_FRAME_NAME, rospy.Time())
intermediate_transform = Transform()
# create a virtual fram between the feet, this will be the center of the rotation
intermediate_transform.translation.x = (
left_foot_world.transform.translation.x + right_foot_world.transform.translation.x)/2.
intermediate_transform.translation.y = (
left_foot_world.transform.translation.y + right_foot_world.transform.translation.y)/2.
intermediate_transform.translation.z = (
left_foot_world.transform.translation.z + right_foot_world.transform.translation.z)/2.
# here we assume that feet have the same orientation so we can pick arbitrary left or right
intermediate_transform.rotation = left_foot_world.transform.rotation
left_footstep.location = left_foot_world.transform.translation
right_footstep.location = right_foot_world.transform.translation
# define the turning radius
radius = sqrt(
(
right_foot_world.transform.translation.x -
left_foot_world.transform.translation.x
)**2 + (
right_foot_world.transform.translation.y -
left_foot_world.transform.translation.y
)**2) / 2.
left_offset = [-radius*sin(yaw), radius*(1-cos(yaw)), 0]
right_offset = [radius*sin(yaw), -radius*(1-cos(yaw)), 0]
intermediate_euler = euler_from_quaternion([
intermediate_transform.rotation.x,
intermediate_transform.rotation.y,
intermediate_transform.rotation.z,
intermediate_transform.rotation.w])
resulting_quat = quaternion_from_euler(
intermediate_euler[0], intermediate_euler[1],
intermediate_euler[2] + yaw)
rot = quaternion_matrix([
resulting_quat[0], resulting_quat[1], resulting_quat[2], resulting_quat[3]])
left_transformedOffset = numpy.dot(rot[0:3, 0:3], left_offset)
right_transformedOffset = numpy.dot(rot[0:3, 0:3], right_offset)
quat_final = Quaternion(
resulting_quat[0], resulting_quat[1], resulting_quat[2], resulting_quat[3])
left_footstep.location.x += left_transformedOffset[0]
left_footstep.location.y += left_transformedOffset[1]
left_footstep.location.z += left_transformedOffset[2]
left_footstep.orientation = quat_final
right_footstep.location.x += right_transformedOffset[0]
right_footstep.location.y += right_transformedOffset[1]
right_footstep.location.z += right_transformedOffset[2]
right_footstep.orientation = quat_final
if yaw > 0:
return [left_footstep, right_footstep]
else:
return [right_footstep, left_footstep]
def createRotationFootStepList(self, yaw):
left_footstep = FootstepDataRosMessage()
left_footstep.robot_side = FootstepDataRosMessage.LEFT
right_footstep = FootstepDataRosMessage()
right_footstep.robot_side = FootstepDataRosMessage.RIGHT
left_foot_world = self.tfBuffer.lookup_transform(
'world', self.LEFT_FOOT_FRAME_NAME, rospy.Time())
right_foot_world = self.tfBuffer.lookup_transform(
'world', self.RIGHT_FOOT_FRAME_NAME, rospy.Time())
intermediate_transform = Transform()
# create a virtual fram between the feet, this will be the center of the rotation
intermediate_transform.translation.x = (
left_foot_world.transform.translation.x +
right_foot_world.transform.translation.x) / 2.
intermediate_transform.translation.y = (
left_foot_world.transform.translation.y +
right_foot_world.transform.translation.y) / 2.
intermediate_transform.translation.z = (
left_foot_world.transform.translation.z +
right_foot_world.transform.translation.z) / 2.
# here we assume that feet have the same orientation so we can pick arbitrary left or right
#intermediate_transform.rotation = left_foot_world.transform.rotation # Get Pelvis orientation
# We will use the pelvis's rotation as the initial frame of reference
pelvis_world = self.tfBuffer.lookup_transform('world',
self.PELVIS_FRAME_NAME,
rospy.Time())
quat_pelvis_world = pelvis_world.transform.rotation
quat_pelvis = numpy.array([
quat_pelvis_world.w, quat_pelvis_world.x, quat_pelvis_world.y,
quat_pelvis_world.z
])
quat_to_use = self.get_pelvis_xy_coplanar_quat(quat_pelvis)
quat = Quaternion()
quat.w = quat_to_use[0]
quat.x = quat_to_use[1]
quat.y = quat_to_use[2]
quat.z = quat_to_use[3]
intermediate_transform.rotation = quat
left_footstep.location = left_foot_world.transform.translation
right_footstep.location = right_foot_world.transform.translation
# define the turning radius
radius = sqrt((right_foot_world.transform.translation.x -
left_foot_world.transform.translation.x)**2 +
(right_foot_world.transform.translation.y -
left_foot_world.transform.translation.y)**2) / 2.
left_offset = [-radius * sin(yaw), radius * (1 - cos(yaw)), 0]
right_offset = [radius * sin(yaw), -radius * (1 - cos(yaw)), 0]
intermediate_euler = euler_from_quaternion([
intermediate_transform.rotation.x,
intermediate_transform.rotation.y,
intermediate_transform.rotation.z,
intermediate_transform.rotation.w
])
resulting_quat = quaternion_from_euler(intermediate_euler[0],
intermediate_euler[1],
intermediate_euler[2] + yaw)
rot = quaternion_matrix([
resulting_quat[0], resulting_quat[1], resulting_quat[2],
resulting_quat[3]
])
left_transformedOffset = numpy.dot(rot[0:3, 0:3], left_offset)
right_transformedOffset = numpy.dot(rot[0:3, 0:3], right_offset)
quat_final = Quaternion(resulting_quat[0], resulting_quat[1],
resulting_quat[2], resulting_quat[3])
left_footstep.location.x += left_transformedOffset[0]
left_footstep.location.y += left_transformedOffset[1]
if ON_REAL_ROBOT_USE:
left_footstep.location.z = 0.0
else:
left_footstep.location.z = -SOLE_FRAME_Z_OFFSET #+= left_transformedOffset[2]
left_footstep.orientation = quat_final
right_footstep.location.x += right_transformedOffset[0]
right_footstep.location.y += right_transformedOffset[1]
if ON_REAL_ROBOT_USE:
right_footstep.location.z = 0.0
else:
right_footstep.location.z = -SOLE_FRAME_Z_OFFSET #+= right_transformedOffset[2]
right_footstep.orientation = quat_final
if yaw > 0:
return [left_footstep, right_footstep]
else:
return [right_footstep, left_footstep]
