00001
00002 import roslib; roslib.load_manifest('halfsteps_pattern_generator')
00003 import rospy
00004 from halfsteps_pattern_generator.GetPathClient \
00005 import HalfStepPatternGeneratorClient
00006 import halfsteps_pattern_generator.msg
00007
00008 if __name__ == "__main__":
00009 print "Calling getPath service..."
00010 try:
00011 stepLength = 0.
00012
00013 client = HalfStepPatternGeneratorClient()
00014 client.initial_left_foot_position.position.y = +0.095
00015 client.initial_right_foot_position.position.y = -0.095
00016 client.initial_center_of_mass_position.z = 0.8
00017 client.final_left_foot_position.position.y = +0.095
00018 client.final_right_foot_position.position.y = -0.095
00019 client.final_center_of_mass_position.z = 0.8
00020 client.start_with_left_foot = True
00021 client.footprints = []
00022
00023 client.final_left_foot_position.position.x = 3 * 0.25
00024 client.final_right_foot_position.position.x = 3 * 0.25
00025 client.final_center_of_mass_position.x = 3 * 0.25
00026
00027 def makeFootprint(x, y, slideUp = -0.5, slideDown = -0.5):
00028 footprint = halfsteps_pattern_generator.msg.Footprint()
00029 footprint.footprint.beginTime.secs = 0
00030 footprint.footprint.beginTime.nsecs = 0
00031 footprint.footprint.duration.secs = 0.
00032 footprint.footprint.duration.nsecs = 1. * 1e9
00033 footprint.footprint.x = x
00034 footprint.footprint.y = y
00035 footprint.footprint.theta = 0.
00036 footprint.slideUp = slideUp
00037 footprint.slideDown = slideDown
00038 footprint.horizontalDistance = 0.19
00039 footprint.stepHeight = 0.1
00040
00041 print("{0} {1}".format(footprint.footprint.x,
00042 footprint.footprint.y))
00043 return footprint
00044
00045 client.footprints.append(makeFootprint(1 * stepLength, +0.095,
00046 slideUp = 0.))
00047 client.footprints.append(makeFootprint(1 * stepLength, -0.095))
00048
00049 client.footprints.append(makeFootprint(2 * stepLength, +0.095))
00050 client.footprints.append(makeFootprint(2 * stepLength, -0.095))
00051
00052 client.footprints.append(makeFootprint(3 * stepLength, +0.095))
00053 client.footprints.append(makeFootprint(3 * stepLength, -0.095,
00054 slideDown = 0.))
00055
00056 if not not client():
00057 print("Service call succeed")
00058 else:
00059 print("Service call failed")
00060
00061 except rospy.ServiceException, e:
00062 print "Service call failed: %s"%e
