def create_setup_footsteps(adjust_x=0.0, left_y=-0.08, stance_width=0.18):
footsteps = []
rightstep = FootstepDataRosMessage()
rightstep.robot_side = RIGHT
rightstep.orientation = deepcopy(rf_start_orientation)
rightstep.location = deepcopy(rf_start_position)
rightstep.trajectory_type = rightstep.PUSH_RECOVERY
rightstep.origin = rightstep.AT_SOLE_FRAME
rightstep.location.x = adjust_x
rightstep.location.y = left_y - stance_width
zarj_tf.transform_from_world(rightstep.location)
footsteps.append(rightstep)
leftstep = FootstepDataRosMessage()
leftstep.robot_side = LEFT
leftstep.orientation = deepcopy(lf_start_orientation)
leftstep.location = deepcopy(lf_start_position)
leftstep.trajectory_type = leftstep.PUSH_RECOVERY
leftstep.origin = leftstep.AT_SOLE_FRAME
leftstep.location.x = adjust_x
leftstep.location.y = left_y
zarj_tf.transform_from_world(leftstep.location)
footsteps.append(leftstep)
return footsteps
def create_one_footstep(side, offset):
global lf_start_position
global lf_start_orientation
global rf_start_position
global rf_start_orientation
footstep = FootstepDataRosMessage()
footstep.robot_side = side
if side == LEFT:
footstep.orientation = deepcopy(lf_start_orientation)
footstep.location = deepcopy(lf_start_position)
else:
footstep.orientation = deepcopy(rf_start_orientation)
footstep.location = deepcopy(rf_start_position)
# JPW There are further parameters on feet that we
# should be able to play with. This is supposedly the 'fast'
# trajectory
footstep.trajectory_type = footstep.PUSH_RECOVERY
footstep.location.x += offset[0]
footstep.location.y += offset[1]
footstep.location.z += offset[2]
return footstep
def create_one_abs_footstep(self, side, location, orientation):
""" Simple create of a footstep to move a foot to a precise location."""
footstep = FootstepDataRosMessage()
footstep.robot_side = side
footstep.location = location
footstep.orientation = orientation
footstep.trajectory_type = FootstepDataRosMessage.DEFAULT
# Create report of what we are doing relative to left foot.
cur_pos = self.lf_start_position
cur_angle = compute_rot_angle(self.lf_start_orientation)
cur_pos_diff = [location.x - cur_pos.x, location.y - cur_pos.y, 0]
lf_rot_pos_diff = self.undo_transform_offset(cur_pos_diff)
new_angle = compute_rot_angle(orientation)
if side == FootstepDataRosMessage.LEFT: footstep_type = "left"
else: footstep_type = "right"
log("Moving {} foot relative to (starting) left foot to position {} and angle {}."
.format(footstep_type, fmt(lf_rot_pos_diff),
fmt(new_angle - cur_angle)))
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
