def forward(self, distance, square_up=True, start_foot=LEFT):
""" Walk forward the given distance """
msg = FootstepDataListRosMessage()
msg.default_transfer_time = self.transfer_time
msg.default_swing_time = self.swing_time
stride = self.stride
if distance < 0 and stride > 0.3:
log('Walking backward exceeds safe stride')
stride = 0.3
msg.execution_mode = msg.OVERRIDE # Override means replace others
msg.unique_id = self.zarj.uid
# TODO - this may be nice to rethink
self.lookup_feet(report=True)
msg.footstep_data_list = self.create_footsteps(distance, start_foot,
square_up, self.stride)
self.steps = 0
self.planned_steps = len(msg.footstep_data_list)
self.walk_failure = None
self.start_walk = rospy.get_time()
self.footstep_list_publisher.publish(msg)
log('Move ' + fmt(distance) + ' FootstepDataList: uid ' +
str(msg.unique_id))
def fix_stance(self):
"""
fix stance
"""
self.lookup_feet(report=True)
distance_between_feet = sqrt(
(self.rf_start_position.x - self.lf_start_position.x)**2 +
(self.rf_start_position.y - self.lf_start_position.y)**2)
if abs(distance_between_feet - self.stance_width) > 0.01:
distance = self.stance_width - distance_between_feet
else:
return
msg = FootstepDataListRosMessage()
msg.default_transfer_time = self.DEFAULT_TRANSFER_TIME
msg.default_swing_time = self.DEFAULT_SWING_TIME
msg.execution_mode = msg.OVERRIDE # Override means replace others
msg.unique_id = self.zarj.uid
start_foot = FootstepDataRosMessage.LEFT
msg.footstep_data_list = [
self.create_one_footstep(start_foot, [0.0, distance, 0.0])
]
self.steps = 0
self.planned_steps = len(msg.footstep_data_list)
self.walk_failure = None
self.start_walk = rospy.get_time()
self.footstep_list_publisher.publish(msg)
log('FootstepDataList: uid ' + str(msg.unique_id))
def side(self, distance):
"""
Step to the side without changing orientation.
Positive steps to the left, negative steps to the right
"""
msg = FootstepDataListRosMessage()
msg.default_transfer_time = self.DEFAULT_TRANSFER_TIME
msg.default_swing_time = self.DEFAULT_SWING_TIME
msg.execution_mode = msg.OVERRIDE # Override means replace others
msg.unique_id = self.zarj.uid
self.lookup_feet(report=True)
if distance < 0:
start_foot = FootstepDataRosMessage.RIGHT
else:
start_foot = FootstepDataRosMessage.LEFT
msg.footstep_data_list = self.create_side_steps(
distance, start_foot, self.SIDE_STRIDE)
self.steps = 0
self.planned_steps = len(msg.footstep_data_list)
self.walk_failure = None
self.start_walk = rospy.get_time()
self.footstep_list_publisher.publish(msg)
log('FootstepDataList: uid ' + str(msg.unique_id))
def getEmptyFootsetListMsg(self):
msg = FootstepDataListRosMessage()
msg.default_transfer_time = 1.5
msg.default_swing_time = 1.5
msg.execution_mode = 0
msg.unique_id = -1
return msg
def boxStep():
msg = FootstepDataListRosMessage()
msg.unique_id = -1
msg.default_transfer_time = 1.5
msg.default_swing_time = 1.5
# walk forward starting LEFT
msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.4, 0.0, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.8, 0.0, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.8, 0.0, 0.0]))
# walk left starting LEFT
msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.8, 0.3, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.8, 0.3, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.8, 0.6, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.8, 0.6, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.8, 0.8, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.8, 0.8, 0.0]))
# walk back starting LEFT
msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.4, 0.8, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.0, 0.8, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.0, 0.8, 0.0]))
# walk right starting RIGHT
msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.0, 0.6, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.0, 0.6, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.0, 0.3, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.0, 0.3, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.0, 0.0, 0.0]))
msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.0, 0.0, 0.0]))
footStepListPublisher.publish(msg)
print 'box stepping...'
waitForFootsteps(len(msg.footstep_data_list))
def getFootFootstepMsg(self, foot):
#msg = self.getEmptyFootsetListMsg()
msg = FootstepDataListRosMessage()
msg.default_transfer_time = 0.3 #was transfer_time 1.5
msg.default_swing_time = 0.7 #was swing_time 1.5
msg.execution_mode = 0
msg.unique_id = -1
ROSleft = FootstepDataRosMessage.LEFT
ROSright = FootstepDataRosMessage.RIGHT
footstep = FootstepDataRosMessage()
step_side = ROSleft if foot.side == Foot.LEFT else ROSright
footstep.robot_side = step_side
#loc,rot = getROSloc(side)
rot = quaternion_from_euler(0, 0, foot.t)
footstep.orientation.x = rot[0]
footstep.orientation.y = rot[1]
footstep.orientation.z = rot[2]
footstep.orientation.w = rot[3]
footstep.location.x = foot.x
footstep.location.y = foot.y
footstep.location.z = self.recentz
msg.footstep_data_list.append(footstep)
return msg
def make_steps(step_set):
msg = FootstepDataListRosMessage()
msg.default_transfer_time = 1.0
msg.default_swing_time = 1.0
reference_pose, steps = step_set
msg.footstep_data_list = [make_footstep(reference_pose, *step) for step in steps]
msg.unique_id = 1
return msg
def getEmptyFootsetListMsg(self):
msg = FootstepDataListRosMessage()
if ON_REAL_ROBOT_USE:
msg.default_transfer_duration = 1.5
msg.default_swing_duration = 1.5
else:
msg.default_transfer_time = 1.5
msg.default_swing_time = 1.5
msg.execution_mode = 0
msg.unique_id = -1
return msg
def getEmptyFootsetListMsg(self):
msg = FootstepDataListRosMessage()
if ON_REAL_ROBOT_USE:
msg.execution_timing = msg.CONTROL_DURATIONS
msg.default_transfer_duration = 0.0 # IHMC default param
msg.default_swing_duration = 0.0 # IHMC default param
msg.final_transfer_duration = -9.81
else:
msg.default_transfer_time = 0.0 #3.0#1.5
msg.default_swing_time = 0.0 #3.0#1.5
msg.execution_mode = 0
msg.unique_id = -1
return msg
def step_up(self, distance, rise):
""" Step up a step """
msg = FootstepDataListRosMessage()
msg.default_transfer_time = self.transfer_time
msg.default_swing_time = self.swing_time
msg.execution_mode = msg.OVERRIDE # Override means replace others
msg.unique_id = self.zarj.uid
self.lookup_feet()
start_foot = FootstepDataRosMessage.LEFT
msg.footstep_data_list = self.create_up_steps(distance, rise,
start_foot,
self.DEFAULT_STRIDE)
self.steps = 0
self.planned_steps = len(msg.footstep_data_list)
self.walk_failure = None
self.start_walk = rospy.get_time()
self.footstep_list_publisher.publish(msg)
log('FootstepDataList: uid ' + str(msg.unique_id))
def walk_to(self, point):
""" Shuffle to a given point, point is given for the left foot """
msg = FootstepDataListRosMessage()
msg.default_transfer_time = self.DEFAULT_TRANSFER_TIME
msg.default_swing_time = self.DEFAULT_SWING_TIME
msg.execution_mode = msg.OVERRIDE # Override means replace others
msg.unique_id = self.zarj.uid
self.lookup_feet()
delta = Vector3(point.x - self.lf_start_position.x,
point.y - self.lf_start_position.y, 0)
start_foot = self._find_first_xy_foot(point)
msg.footstep_data_list = self.create_xy_steps(delta, start_foot, 0.15)
self.steps = 0
self.planned_steps = len(msg.footstep_data_list)
self.walk_failure = None
self.start_walk = rospy.get_time()
self.footstep_list_publisher.publish(msg)
log('FootstepDataList: uid ' + str(msg.unique_id))
def move_foot(self, foot_type, x, y, rotate):
"""
Move a foot, relative to the current left foot frame and rotate by a particular angle.
This code is meant to be used for setting up messy test scenarios that can be fixed by the
*turn* method.
"""
self.lookup_feet(report=True)
left_ft_angle = compute_rot_angle(self.lf_start_orientation)
left_ft_rot = quaternion_about_axis(radians(left_ft_angle), [0, 0, 1])
offset = rotate_v([x, y, 0], left_ft_rot)
if foot_type == "left":
ft_loc = msg_v_to_v(self.lf_start_position)
foot_choice = LEFT
else:
ft_loc = msg_v_to_v(self.rf_start_position)
foot_choice = RIGHT
target = add_v(ft_loc, offset)
foot_rotation = quaternion_about_axis(radians(left_ft_angle + rotate),
[0, 0, 1])
msg = FootstepDataListRosMessage()
msg.footstep_data_list = [
self.create_one_abs_footstep(foot_choice, v_to_msg_v(target),
q_to_msg_q(foot_rotation))
]
msg.default_transfer_time = self.transfer_time
msg.default_swing_time = self.swing_time
msg.execution_mode = msg.OVERRIDE # Override means replace others
msg.unique_id = self.zarj.uid
self.steps = 0
self.planned_steps = len(msg.footstep_data_list)
self.walk_failure = None
self.start_walk = rospy.get_time()
self.footstep_list_publisher.publish(msg)
log("Moving {} foot by x={}, y={}, and rotating to {}".format(
foot_type, fmt(x), fmt(y), fmt(left_ft_angle + rotate)))
while not self.walk_is_done():
rospy.sleep(0.2)
rospy.sleep(0.2)
def walkTest():
msg = FootstepDataListRosMessage()
msg.default_transfer_time = 1.5
msg.default_swing_time = 1.5
msg.execution_mode = 0
msg.unique_id = -1
# walk forward starting LEFT
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.LEFT, [0.2, 0.0, 0.0]))
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.RIGHT, [0.4, 0.0, 0.0]))
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.LEFT, [0.6, 0.0, 0.0]))
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.RIGHT, [0.8, 0.0, 0.0]))
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.LEFT, [1.0, 0.0, 0.0]))
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.RIGHT, [1.2, 0.0, 0.0]))
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.LEFT, [1.4, 0.0, 0.0]))
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.RIGHT, [1.6, 0.0, 0.0]))
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.LEFT, [1.8, 0.0, 0.0]))
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.RIGHT, [2.0, 0.0, 0.0]))
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.LEFT, [2.2, 0.0, 0.0]))
msg.footstep_data_list.append(
createFootStepOffset(FootstepDataRosMessage.RIGHT, [2.4, 0.0, 0.0]))
footStepListPublisher.publish(msg)
rospy.loginfo('walk forward...')
waitForFootsteps(len(msg.footstep_data_list))
def turn(self,
orig_angle,
snap_to=15,
small_forward=None,
report=True,
align_to_snap=False):
""" Turn to the given angle. Note for a larger angle that we will take one step
a bit to the side, before returning, so be aware that we need
some space to turn. Angle is positive for counter clockwise. The align_to_snap, if set to true,
essentially prevents the feet from rotating around a center, but instead just tries to fix
the current orientation of the feet, using the most forward foot as the basis of this correction. The
problem is that if you the left foot is rotated -5 degrees from optimal on a stair, then
doing a normal +5 degree rotation of the feet to correct it will cause the left foot to backwards by 0.012,
which can cause stair failure. """
self.lookup_feet(report=report)
left_ft_angle = compute_rot_angle(self.lf_start_orientation)
angle = orig_angle
align_angle = left_ft_angle
if snap_to is not None and snap_to > 0.5:
total_angle = left_ft_angle + orig_angle
# Round to nearest snap_to degree multiple.
rounded_angle = round(total_angle / snap_to) * snap_to
snap_diff = rounded_angle - total_angle
angle += snap_diff
if align_to_snap:
align_angle = rounded_angle
log("Applying 'snap_to' of " + fmt(snap_to) +
" to current left foot angle " + fmt(left_ft_angle) +
" creating angle diff " + fmt(snap_diff) +
" to change turn angle " + fmt(orig_angle) +
" to turn angle " + fmt(angle))
log("Small forward={}, align_angle={}".format(
fmt(small_forward), fmt(align_angle)))
if angle > 180:
rospy.logerr("Error: angle cannot exceed 180 degrees")
return
if angle < -180:
rospy.logerr("Error: angle cannot go below -180 degrees")
return
# We use the "left foot dictates forward position" logic.
# In the reference frame of the left foot, we use the left foots forward position
# as the average, instead of averaging it with the right foot. In climbing stairs,
# the left foot has a more reliable relative "forward" position.
lf_to_rf_diffx = self.rf_start_position.x - self.lf_start_position.x
lf_to_rf_diffy = self.rf_start_position.y - self.lf_start_position.y
# View diff relative to left foot.
undo_align_rotation = quaternion_about_axis(radians(-align_angle),
[0, 0, 1])
rf_rel = rotate_v([lf_to_rf_diffx, lf_to_rf_diffy, 0.0],
undo_align_rotation)
# The nice thing about doing math in the left foot frame (with a small adjustement
# for small change in alignment angle), is that the left foot
# has 0,0,0 as its coordinates, making all the math focus on the right foot's
# relative location.
rf_rel_x = rf_rel[0]
rf_rel_y = rf_rel[1]
# See if we are asked to make a small adjustment forward (for making sure we stay stuck up against stairs
# tables, and walls).
# The following conditional test is based on observation and reports of errors, and where the feet
# appear to be when the errors happen.
rel_avg_x = 0
if align_to_snap and small_forward is not None and small_forward > 0.002 and rf_rel_x > small_forward:
# This is the left foot slipped backwards scenario.
log("Choosing right foot as forward position, it is {} more forward, "
"ignoring small_forward".format(fmt(rf_rel_x)))
rel_avg_x = rf_rel_x + 0.003
elif small_forward is not None:
rel_avg_x += small_forward
rel_avg_y = (
0 + rf_rel_y
) / 2.0 # Notice simple averaging logic for relative y location.
# print "lf_start_position=" + str(self.lf_start_position) + ",rf_start_position=" + str(self.rf_start_position)
# print "rel_avg_x=" + str(rel_avg_x) + ",rel_avg_y=" + str(rel_avg_y)
# Transform average back to world coordinates.
align_rotation = quaternion_about_axis(radians(align_angle), [0, 0, 1])
avg_diff_v = rotate_v([rel_avg_x, rel_avg_y, 0], align_rotation)
avgx = self.lf_start_position.x + avg_diff_v[0]
avgy = self.lf_start_position.y + avg_diff_v[1]
# Compute distance between the feet. Useful for figuring out how much we can do without having
# to do a side step.
current_distance = sqrt(rf_rel_x * rf_rel_x + rf_rel_y * rf_rel_y)
distance = self.stance_width # distance from center point between feet # sqrt(xterm + yterm)
correction_factor = 1.02 # Feet end up not quite as far apart as expected by computations.
distance_ratio = current_distance / distance
choose_left = True
if abs(angle) < 10 or align_to_snap:
# Choose first foot to move by which one is further behind.
# The not pivot foot moves first. Pivot foot should be the
# one that is further ahead of the other. On stairs, this can
# make a difference, especially if the left foot slips backwards.
if rf_rel_x > 0:
choose_left = False
elif angle > 0:
choose_left = False
if choose_left:
foot_type = "left"
pivot_foot = FootstepDataRosMessage.LEFT
pivot_angle = compute_rot_angle(self.lf_start_orientation)
pivot_start_pos = self.lf_start_position
not_pivot_angle = compute_rot_angle(self.rf_start_orientation)
not_pivot_start_pos = self.rf_start_position
foot_rel_offset = [0, -distance * correction_factor, 0]
sidestep_rel_offset = [
-self.FOOT_LENGTH / 2.0, -distance * correction_factor, 0
]
else:
foot_type = "right"
pivot_foot = FootstepDataRosMessage.RIGHT
pivot_angle = compute_rot_angle(self.rf_start_orientation)
pivot_start_pos = self.rf_start_position
not_pivot_angle = compute_rot_angle(self.lf_start_orientation)
not_pivot_start_pos = self.lf_start_position
foot_rel_offset = [0, distance * correction_factor, 0]
sidestep_rel_offset = [
-self.FOOT_LENGTH / 2.0, distance * correction_factor, 0
]
# print "pivot_angle=" + str(pivot_angle) + ",not_pivot_angle=" + str(not_pivot_angle)
msg = FootstepDataListRosMessage()
msg.default_transfer_time = self.transfer_time
msg.default_swing_time = self.swing_time
msg.execution_mode = msg.OVERRIDE # Override means replace others
msg.unique_id = self.zarj.uid
total_rot_q = quaternion_about_axis(radians(angle + left_ft_angle),
[0, 0, 1])
abs_offset = rotate_v(foot_rel_offset, total_rot_q)
xoffset = abs_offset[0]
yoffset = abs_offset[1]
xo2 = xoffset / 2
yo2 = yoffset / 2
# Compute location of where we want to send the pivot and not pivot feet.
pivot_dest = [avgx - xo2, avgy - yo2]
not_pivot_dest = [avgx + xo2, avgy + yo2]
# Compute where we need to put the sidestep if we end up using it.
sidestep_offset_v = self.transform_left_offset(sidestep_rel_offset)
sidestep_abs_v = add_v(sidestep_offset_v,
msg_v_to_v(not_pivot_start_pos))
sidestep_abs_msg_v = v_to_msg_v(sidestep_abs_v)
sidestep_msg_q = q_to_msg_q(
quaternion_about_axis(radians(angle / 2 + left_ft_angle),
[0, 0, 1]))
# Convert our locations and angles into parameters that ROS can take. Note that we are
# applying the same rotation to both feet. Note that if align_to_angle is true
# then angle + left_ft_angle is the same as the align_angle.
both_feet_msg_q = q_to_msg_q(
quaternion_about_axis(radians(angle + left_ft_angle), [0, 0, 1]))
pivot_dest_msg_v = Vector3(pivot_dest[0], pivot_dest[1],
pivot_start_pos.z)
not_pivot_dest_msg_v = Vector3(not_pivot_dest[0], not_pivot_dest[1],
not_pivot_start_pos.z)
# For reporting and checking to see if we are really making much of a move.
pivot_offset = [
pivot_dest[0] - pivot_start_pos.x,
pivot_dest[1] - pivot_start_pos.y
]
not_pivot_offset = [
not_pivot_dest[0] - not_pivot_start_pos.x,
not_pivot_dest[1] - not_pivot_start_pos.y
]
def is_small(d):
return -0.015 < d[0] < 0.015 and -0.015 < d[1] < 0.015
# print "pivot_offset=" + str(pivot_offset) + ",not_pivot_offset=" + str(not_pivot_offset)
if -0.3 < angle < 0.3 and is_small(pivot_offset) and is_small(
not_pivot_offset):
log("Skipping turn, because the turn is likely to introduce as much error as "
"would be removed by attempted move.")
self.steps = 0
self.planned_steps = 0
return
footsteps = []
# Make the footsteps.
cap_angle = 5 # Always allow moves of less than five degrees to be done in place.
if current_distance > .19 and current_distance < .24:
cap_angle = 15
elif current_distance >= .24:
cap_angle = 50
# If the angle is large, we are going to assume a turn where one foot is 'planted'
# The other foot rotates and shifts position so that the feet remain
# aligned; their heels should form a line.
# That requires that we do two things - first, make sure that the centers
# of our feet stay the same distance apart, and that the second foot
# is moved to keep them aligned
angle_diff = not_pivot_angle - pivot_angle
angle_check = abs(angle_diff) + abs(angle)
large_angle = not align_to_snap and (angle_check > cap_angle
or angle_check < -cap_angle)
log('Turn angle ' + fmt(angle) + ' from angle ' + fmt(pivot_angle) +
' pivot on ' + foot_type + ' foot, pivot dest ' + fmt(pivot_dest) +
", not pivot dest " + fmt(not_pivot_dest))
log('Doing side step=' + fmt(large_angle) + ', foot angle diff ' +
fmt(angle_diff) + ', feet distance to desired ratio is ' +
fmt(distance_ratio))
if large_angle:
footsteps.append(
self.create_one_abs_footstep(invert_foot(pivot_foot),
sidestep_abs_msg_v,
sidestep_msg_q))
footsteps.append(
self.create_one_abs_footstep(pivot_foot, pivot_dest_msg_v,
both_feet_msg_q))
footsteps.append(
self.create_one_abs_footstep(invert_foot(pivot_foot),
not_pivot_dest_msg_v,
both_feet_msg_q))
if not large_angle:
footsteps.append(
self.create_one_abs_footstep(pivot_foot, pivot_dest_msg_v,
both_feet_msg_q))
msg.footstep_data_list = footsteps
self.steps = 0
self.planned_steps = len(msg.footstep_data_list)
self.walk_failure = None
self.start_walk = rospy.get_time()
self.footstep_list_publisher.publish(msg)
log('Turn ' + fmt(angle) + ' FootstepDataList: uid ' +
str(msg.unique_id))