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