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