def boxStep(): msg = FootstepDataListRosMessage() msg.transfer_time = 1.5 msg.swing_time = 1.5 msg.execution_mode = 0 msg.unique_id = -1 # walk forward starting LEFT msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.2, 0.0, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.4, 0.0, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.4, 0.0, 0.0])) # walk left starting LEFT msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.4, 0.2, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.4, 0.2, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.4, 0.4, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.4, 0.4, 0.0])) # walk back starting LEFT msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.2, 0.4, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.0, 0.4, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.0, 0.4, 0.0])) # walk right starting RIGHT msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.0, 0.2, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.0, 0.2, 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) rospy.loginfo('box stepping...') waitForFootsteps(len(msg.footstep_data_list))
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 getEmptyFootsetListMsg(self): msg = FootstepDataListRosMessage() msg.transfer_time = 1.5 msg.swing_time = 1.5 msg.execution_mode = 0 msg.unique_id = -1 return msg
def createFootStepCmd(): global cmdx,cmdy,cmdn,cmdd,cmds,cmde,cmdl global tfBuffer, tfListener, stepCounter msg = FootstepDataListRosMessage() msg.transfer_time = float(cmdd) msg.swing_time = float(cmds) msg.execution_mode = 0 msg.unique_id = -1 # walk forward starting RIGHT if y is negative # each full step must be <0.75x and <0.2y startingside = RIGHT if (cmdy < 0.0) else LEFT halfsteps = max(cmdn,1) #print( halfsteps ) halfsteps = int(math.ceil(abs(cmdx)/0.6)) if (abs(cmdx)/halfsteps > 0.75) else halfsteps #print( halfsteps ) halfsteps = int(math.ceil(abs(cmdy)/0.2)) if (abs(cmdy)/halfsteps > 0.25) else halfsteps stridex = cmdx/halfsteps stridey = cmdy/halfsteps p1 = "n:%2d dx:%6.3f dy:%6.3f" % ( halfsteps, stridex, stridey ) p2 = " l/r:%2d dual:%6.3f swing:%6.3f" % ( startingside, cmdd, cmds) #print( p1 + p2) msg.footstep_data_list.append(createFootStepOffset( startingside, [stridex, 2.0*stridey, 0.0])) for step in range(1,halfsteps-1): side = (startingside+step) % 2 msg.footstep_data_list.append(createFootStepOffset(side, [stridex*(step+1), 2.0*stridey*step, 0.0])) if halfsteps > 1: step = halfsteps -1 side = (startingside+step) % 2 msg.footstep_data_list.append(createFootStepOffset(side, [stridex*(step+1), cmdy, 0.0])) if stridex != 0.0: msg.footstep_data_list.append(createFootStepOffset((startingside+halfsteps)%2, [cmdx, cmdy, 0.0])) return msg,p1+p2
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 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 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 walkThroughDoor(self): msg = FootstepDataListRosMessage() msg.transfer_time = 1.3 msg.swing_time = 1.3 msg.execution_mode = 0 # This is override mode, as opposed to 1 which is "queue mode" msg.unique_id = -1 # walk forward starting LEFT msg.footstep_data_list.append(self.createFootStepOffset(LEFT, [0.2, 0.0, 0.0])) msg.footstep_data_list.append(self.createFootStepOffset(RIGHT, [0.4, 0.0, 0.0])) msg.footstep_data_list.append(self.createFootStepOffset(LEFT, [0.6, 0.0, 0.0])) msg.footstep_data_list.append(self.createFootStepOffset(RIGHT, [0.8, 0.0, 0.0])) msg.footstep_data_list.append(self.createFootStepOffset(LEFT, [1.0, 0.0, 0.0])) msg.footstep_data_list.append(self.createFootStepOffset(RIGHT, [1.2, 0.0, 0.0])) msg.footstep_data_list.append(self.createFootStepOffset(LEFT, [1.4, 0.0, 0.0])) msg.footstep_data_list.append(self.createFootStepOffset(RIGHT, [1.6, 0.0, 0.0])) msg.footstep_data_list.append(self.createFootStepOffset(LEFT, [1.8, 0.0, 0.0])) msg.footstep_data_list.append(self.createFootStepOffset(RIGHT, [2.0, 0.0, 0.0])) #msg.footstep_data_list.append(self.createFootStepOffset(LEFT, [2.2, 0.0, 0.0])) #msg.footstep_data_list.append(self.createFootStepOffset(RIGHT, [2.4, 0.0, 0.0])) #msg.footstep_data_list.append(self.createFootStepOffset(LEFT, [2.6, 0.0, 0.0])) self.footStepListPublisher.publish(msg) rospy.loginfo('walking through door...') self.waitForFootsteps(len(msg.footstep_data_list))
def getEmptyFootsetListMsg(self): 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.transfer_time = 0.9 #was 1.5 #msg.swing_time = 0.7 #was 1.5 msg.execution_mode = 0 msg.unique_id = -1 return msg
def rightStep(self): msg = FootstepDataListRosMessage() msg.transfer_time = 1.3 msg.swing_time = 1.3 msg.execution_mode = 0 msg.unique_id = -1 msg.footstep_data_list.append(self.createFootStepOffset(RIGHT, [0.2, 0.0, 0.0])) self.footStepListPublisher.publish(msg) rospy.loginfo('stepping forward with right...') self.waitForFootsteps(len(msg.footstep_data_list))
def leftFullStep(self, forwardOffset, transfer_time, swing_time): msg = FootstepDataListRosMessage() msg.transfer_time = transfer_time #0.75 msg.swing_time = swing_time #0.85 msg.execution_mode = 0 msg.unique_id = -1 msg.footstep_data_list.append(self.createFootStepOffset(LEFT, [forwardOffset, 0.0, 0.0])) self.footStepListPublisher.publish(msg) rospy.loginfo('stepping forward with left...') self.waitForFootsteps(len(msg.footstep_data_list))
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 walk(transfer_time, swing_time, footsteps): msg = FootstepDataListRosMessage() msg.transfer_time = transfer_time msg.swing_time = swing_time msg.execution_mode = msg.OVERRIDE # Override means replace others msg.unique_id = uid() msg.footstep_data_list = footsteps footStepListPublisher.publish(msg) rospy.loginfo('FootstepDataList: uid' + str(msg.unique_id))
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 stepInPlace(): msg = FootstepDataListRosMessage() msg.transfer_time = 1.5 msg.swing_time = 1.5 msg.execution_mode = 0 msg.unique_id = -1 msg.footstep_data_list.append(createFootStepInPlace(LEFT)) msg.footstep_data_list.append(createFootStepInPlace(RIGHT)) msg.footstep_data_list.append(createFootStepInPlace(LEFT)) msg.footstep_data_list.append(createFootStepInPlace(RIGHT)) footStepListPublisher.publish(msg) rospy.loginfo('walking in place...') waitForFootsteps(len(msg.footstep_data_list))
def createBackStepCmd(): global cmdx,cmdy,cmdn,cmdd,cmds,cmde,cmdl global tfBuffer, tfListener, stepCounter msg = FootstepDataListRosMessage() msg.transfer_time = 1.0 msg.swing_time = 1.0 msg.execution_mode = 0 msg.unique_id = -1 p1 = " back:%6.3f width:%6.3f" % ( cmdb, cmdw) offset = (cmdw-0.16)/2.0 msg.footstep_data_list.append(createFootStepOffset( LEFT, [-cmdb, offset, 0.0])) msg.footstep_data_list.append(createFootStepOffset( RIGHT, [-cmdb, -offset, 0.0])) return msg,p1
def setFeet(both_x=0.15, right_y=-0.12, foot_separation=0.25): #both_x = 0.02, right_y=-0.12, foot_separation=0.25 rospy.loginfo('Setting feet to fixed position.') right_footstep = FootstepDataRosMessage() right_footstep.robot_side = RIGHT right_footstep.orientation = FOOT_ORIENTATION right_footstep.location = Vector3(x=both_x, y=right_y, z=0) left_footstep = FootstepDataRosMessage() left_footstep.robot_side = LEFT left_footstep.orientation = FOOT_ORIENTATION left_footstep.location = Vector3(x=both_x, y=right_y + foot_separation, z=0) right_current = createFootStepInPlace(RIGHT) if right_current.location.y > right_y: first_footstep = right_footstep second_footstep = left_footstep else: first_footstep = left_footstep second_footstep = right_footstep msg = FootstepDataListRosMessage() # ----- Default Value: 1.5 msg.transfer_time = 1.5 msg.swing_time = 1.5 msg.execution_mode = FootstepDataListRosMessage.OVERRIDE msg.unique_id = rospy.Time.now().nsecs msg.footstep_data_list.append(first_footstep) footStepListPublisher.publish(msg) waitForFootstepCompletion() msg.footstep_data_list[:] = [] msg.unique_id = rospy.Time.now().nsecs msg.footstep_data_list.append(second_footstep) footStepListPublisher.publish(msg) waitForFootstepCompletion() msg.footstep_data_list[:] = [] return left_footstep, right_footstep
def walkThru(): msg = FootstepDataListRosMessage() msg.transfer_time = 0.4 # demo val 1.5 msg.swing_time = 0.4 # demo val 1.5 msg.execution_mode = 0 msg.unique_id = -1 # walk forward starting LEFT #msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.2, 0.1, 0.0])) #msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.25, 0.00, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.37, 0.1, 0.0])) #msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.6, 0.0, 0.05])) #msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.8, 0.0, 0.05])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.6, 0.02, 0.02])) msg.footstep_data_list.append( createFootStepOffset(RIGHT, [0.78, 0.2, 0.02])) msg.footstep_data_list.append( createFootStepOffset(LEFT, [1.0, 0.03, -0.01])) msg.footstep_data_list.append( createFootStepOffset(RIGHT, [1.22, 0.01, -0.01])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [1.44, 0.03, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [1.68, 0.1, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [1.9, 0.03, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [2.12, 0.2, 0.0])) #msg.footstep_data_list.append(createFootStepOffset(LEFT, [2.0, 0.03, 0.0])) ''' msg.footstep_data_list.append(createFootStepOffset(RIGHT, [2.25, 0.0, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [2.45, 0.0, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [2.65, 0.0, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [2.75, 0.0, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [2.82, 0.0, 0.0])) ''' footStepListPublisher.publish(msg) rospy.loginfo('walk forward...') waitForFootsteps(len(msg.footstep_data_list))
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 walkTest(): msg = FootstepDataListRosMessage() msg.transfer_time = 0.38 # demo val 1.5 msg.swing_time = 0.38 # demo val 1.5 msg.execution_mode = 0 msg.unique_id = -1 # walk forward starting LEFT msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.2, 0.0, 0.0])) msg.footstep_data_list.append( createFootStepOffset(RIGHT, [0.4, -0.02, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.6, 0.0, 0.0])) msg.footstep_data_list.append( createFootStepOffset(RIGHT, [0.8, -0.02, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [1.0, 0.0, 0.0])) msg.footstep_data_list.append( createFootStepOffset(RIGHT, [1.2, -0.02, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [1.4, 0.0, 0.0])) msg.footstep_data_list.append( createFootStepOffset(RIGHT, [1.6, -0.02, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [1.8, 0.0, 0.0])) msg.footstep_data_list.append( createFootStepOffset(RIGHT, [2.0, -0.02, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [2.2, 0.0, 0.0])) msg.footstep_data_list.append( createFootStepOffset(RIGHT, [2.4, -0.03, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [2.6, -0.01, 0.0])) msg.footstep_data_list.append( createFootStepOffset(RIGHT, [2.85, -0.25, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [2.8, -0.01, 0.0])) #msg.footstep_data_list.append(createFootStepOffset(RIGHT, [2.8, -0.2, 0.0])) footStepListPublisher.publish(msg) rospy.loginfo('walk forward...') waitForFootsteps(len(msg.footstep_data_list))
def walkThru(x, y): msg = FootstepDataListRosMessage() msg.transfer_time = 0.5 # demo val 1.5 msg.swing_time = 0.5 # demo val 1.5 msg.execution_mode = 0 msg.unique_id = -1 # walk forward starting LEFT ''' msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.0, -0.1, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.0, -0.8, 0.0])) ''' if x < 0 or y < 0: msg.footstep_data_list.append(createFootStepOffset(RIGHT, [y, x, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [y, x, 0.0])) else: msg.footstep_data_list.append(createFootStepOffset(LEFT, [y, x, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [y, x, 0.0])) ''' msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.4, 0.3, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [0.6, 0.28, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [0.8, 0.3, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [1.0, 0.28, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [1.2, 0.3, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [1.4, 0.28, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [1.6, 0.3, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [1.8, 0.28, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [2.0, 0.3, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [2.25, 0.0, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [2.45, 0.0, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [2.65, 0.0, 0.0])) msg.footstep_data_list.append(createFootStepOffset(LEFT, [2.75, 0.0, 0.0])) msg.footstep_data_list.append(createFootStepOffset(RIGHT, [2.82, 0.0, 0.0])) ''' footStepListPublisher.publish(msg) rospy.loginfo('walk forward...') waitForFootsteps(len(msg.footstep_data_list))
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 walkForward(distance, step_size=0.8, transfer_time=0.4, swing_time=0.4, allowed_delta=0.01): #distance, step_size=0.4, transfer_time=0.7, swing_time=0.7, allowed_delta=0.01 msg = FootstepDataListRosMessage() msg.transfer_time = transfer_time msg.swing_time = swing_time # footSeparation = 0.07, x2 left_footstep = createFootStepInPlace(LEFT) left_footstep.orientation = FOOT_ORIENTATION right_footstep = createFootStepInPlace(RIGHT) right_footstep.orientation = FOOT_ORIENTATION # determine final distance relative to right foot target_x = right_footstep.location.x + distance step_count = 0 continue_walking = True is_penultimate = False while continue_walking == True: step_distance = step_size msg.execution_mode = FootstepDataListRosMessage.QUEUE msg.unique_id = rospy.Time.now().nsecs if step_count == 0: rospy.loginfo('Initial step, half-size.') msg.execution_mode = FootstepDataListRosMessage.OVERRIDE step_distance = step_size / 2.0 if is_penultimate == True: rospy.loginfo('Last step.') continue_walking = False if step_count % 2 == 0: left_footstep.location.x += step_distance if left_footstep.location.x >= target_x - allowed_delta: is_penultimate = True left_footstep.location.x = target_x msg.footstep_data_list.append(left_footstep) rospy.loginfo('Step{0}, left'.format(step_count + 1)) else: right_footstep.location.x += step_distance if right_footstep.location.x >= target_x - allowed_delta: is_penultimate = True right_footstep.location.x = target_x msg.footstep_data_list.append(right_footstep) rospy.loginfo('Step{0}, right'.format(step_count + 1)) footStepListPublisher.publish(msg) waitForFootstepCompletion() step_count += 1 rospy.loginfo('Step {0} finished.'.format(step_count)) msg.footstep_data_list[:] = [] rospy.loginfo('Finished walking forward.')
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))
def walkTest(argv): copts = "x:y:n:d:s:h?" cmdx = 0.0 cmdy = 0.0 cmdn = 1 cmdd = 1.5 cmds = 1.5 try: opts, args = getopt.getopt(argv[1:], copts) except getopt.GetoptError: printhelp(argv[0]) sys.exit(2) for opt, arg in opts: if opt in ('-h', '-?'): printhelp(argv[0]) if opt == '-x': cmdx = float(arg) if opt == '-y': cmdy = float(arg) if opt == '-n': cmdn = int(arg) if opt == '-d': cmdd = float(arg) if opt == '-s': cmds = float(arg) msg = FootstepDataListRosMessage() msg.transfer_time = float(cmdd) msg.swing_time = float(cmds) msg.execution_mode = 0 msg.unique_id = -1 # walk forward starting RIGHT if y is negative # each full step must be <0.75x and <0.2y startingside = RIGHT if (cmdy < 0.0) else LEFT halfsteps = max(cmdn, 1) print halfsteps halfsteps = int(math.ceil( abs(cmdx) / 0.6)) if (abs(cmdx) / halfsteps > 0.75) else halfsteps print halfsteps halfsteps = int(math.ceil( abs(cmdy) / 0.2)) if (abs(cmdy) / halfsteps > 0.25) else halfsteps stridex = cmdx / halfsteps stridey = cmdy / halfsteps p1 = "n:%2d dx:%6.3f dy:%6.3f" % (halfsteps, stridex, stridey) p2 = " l/r:%2d dual:%6.3f swing:%6.3f" % (startingside, cmdd, cmds) print p1 + p2 msg.footstep_data_list.append( createFootStepOffset(startingside, [stridex, 2.0 * stridey, 0.0])) for step in range(1, halfsteps - 1): side = (startingside + step) % 2 msg.footstep_data_list.append( createFootStepOffset( side, [stridex * (step + 1), 2.0 * stridey * step, 0.0])) if halfsteps > 1: step = halfsteps - 1 side = (startingside + step) % 2 msg.footstep_data_list.append( createFootStepOffset(side, [stridex * (step + 1), cmdy, 0.0])) if stridex != 0.0: msg.footstep_data_list.append( createFootStepOffset((startingside + halfsteps) % 2, [cmdx, cmdy, 0.0])) footStepListPublisher.publish(msg) rospy.loginfo('walking...') waitForFootsteps(len(msg.footstep_data_list))