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