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