def path_to_trajectory(
path, dt, leg_name, delay=0.5, prefix='stompyleg'):
msg = control_msgs.msg.FollowJointTrajectoryGoal()
msg.trajectory.joint_names.append(
'%s__body_to_%s' % (prefix, leg_name))
msg.trajectory.joint_names.append(
'%s__%s__hip_to_thigh' % (prefix, leg_name))
msg.trajectory.joint_names.append(
'%s__%s__thigh_to_calf_upper' % (prefix, leg_name))
# throw out first point
path.next()
pt = path.next()
t = dt
while pt is not None:
a = leg.inverse(pt[0], pt[1], pt[2])
p = trajectory_msgs.msg.JointTrajectoryPoint()
p.time_from_start = rospy.Duration(t)
p.positions = list(a)
msg.trajectory.points.append(p)
t += dt
pt = path.next()
msg.trajectory.header.stamp = (
rospy.Time.now() + rospy.Duration(delay))
# define tolerances
msg.goal_tolerance
msg.goal_time_tolerance = rospy.Duration(1.0)
return msg
def main():
p = argparse.ArgumentParser()
p.add_argument('-s', '--start', default="", type=str)
p.add_argument('-e', '--end', default="1.8,0.0,0.5", type=str)
p.add_argument('-t', '--time', default=5., type=float)
p.add_argument('-r', '--rate', default=10., type=float)
args = p.parse_args()
rospy.init_node('stompyleg_trace', anonymous=True)
rospy.Subscriber(
"/stompyleg/joint_states",
sensor_msgs.msg.JointState,
lambda data, description=joints.stompyleg_leg_descriptions: joints.
update_joints(data, description))
lc = lambda joint: '/stompyleg/%s_controller/command' % (joint, )
hp = rospy.Publisher(lc('hip'), std_msgs.msg.Float64, queue_size=18)
tp = rospy.Publisher(lc('thigh'), std_msgs.msg.Float64, queue_size=18)
kp = rospy.Publisher(lc('knee'), std_msgs.msg.Float64, queue_size=18)
start = None
if args.start != "":
start = map(float, args.start.split(','))
end = map(float, args.end.split(','))
path = PathTracer(start=start, end=end, time=args.time, rate=args.rate)
pt = None
print("Target: %s" % end)
sleep_time = 1. / args.rate
while not rospy.is_shutdown():
if path.start is None:
# (read out start position)
if joints.joints is not None:
path.start = leg.forward(joints.legs['fl']['hip'],
joints.legs['fl']['thigh'],
joints.legs['fl']['knee'])
print("Found starting position: %s" % (path.start, ))
else:
# compute vector from start to end
# break up vector wrt time and rate
pt = path.next()
if pt is None:
break
print("Moving to: %s" % (pt, ))
h, t, k = leg.inverse(pt[0], pt[1], pt[2])
hp.publish(h)
tp.publish(t)
kp.publish(k)
# start movement...
rospy.sleep(sleep_time)
def path_to_trajectory(path, leg_name, start_time):
#msg = trajectory_msgs.msg.JointTrajectory()
#msg = goal.action_goal.goal
msg = control_msgs.msg.FollowJointTrajectoryGoal()
msg.trajectory.joint_names.append('stompy__body_to_%s' % (leg_name, ))
msg.trajectory.joint_names.append('stompy__%s__hip_to_thigh' %
(leg_name, ))
msg.trajectory.joint_names.append('stompy__%s__thigh_to_calf_upper' %
(leg_name, ))
# throw out first point, as the start is the current position here
path.next()
pt = path.next()
total_time = path.time
dt = total_time / (path.n - 1)
t = dt
while pt is not None:
a = leg.inverse(pt[0], pt[1], pt[2])
p = trajectory_msgs.msg.JointTrajectoryPoint()
p.time_from_start = rospy.Duration(t)
p.positions = list(a)
msg.trajectory.points.append(p)
t += dt
pt = path.next()
# give last point time to settle
#msg.trajectory.points[-1].time_from_start += rospy.Duration(0.5)
msg.trajectory.header.stamp = start_time
#msg.header.stamp = rospy.Time.now() + rospy.Duration(delay)
for n in ('stompy__body_to_%s' % (leg_name, ),
'stompy__%s__hip_to_thigh' % (leg_name, ),
'stompy__%s__thigh_to_calf_upper' % (leg_name, )):
t = control_msgs.msg.JointTolerance()
t.name = n
t.position = 0.1
msg.goal_tolerance.append(t)
msg.goal_time_tolerance = rospy.Duration(1.0)
return msg # goal
def main():
p = argparse.ArgumentParser(
"Move the leg from it's current location to some end point [end]\n"
"break up the movment into smaller linear moves with each sub-move\n"
"only moving a joint at moves delta radians")
#p.add_argument('-s', '--start', default="", type=str)
#p.add_argument('-e', '--end', default="1.8,0.0,0.5", type=str)
p.add_argument('-e',
'--end',
default="0.0,0.0,0.1",
type=str,
help="End point in x,y,z (escape - xs with a leading \\)")
#p.add_argument('-t', '--time', default=5., type=float)
p.add_argument('-p', '--port', default='/dev/ttyACM0')
p.add_argument('-b', '--baud', default=9600, type=int)
#p.add_argument('-d', '--delta', default=numpy.radians(1))
#p.add_argument('-c', '--close', default=numpy.radians(0.5))
p.add_argument('-d',
'--delta',
default=0.03,
type=float,
help="Maximum angle deviation (radians) between subtargets")
p.add_argument(
'-c',
'--close',
default=0.02,
type=float,
help="Acceptable angle deviation before a subtarget is reached")
p.add_argument('-f',
'--fake',
default=False,
action='store_true',
help="Don't read/write the serial port, fake movements")
p.add_argument('-F',
'--force',
default=False,
action='store_true',
help="Force all prompts to yes (DANGEROUS)")
args = p.parse_args()
#conn = serial.Serial(args.port, args.baud)
if args.fake:
conn = None
else:
conn = serial.Serial(args.port, args.baud)
# read in joint angles
# eat first line
if conn is not None:
conn.readline()
a = None
while a is None:
a = read_angles(conn)
hip, thigh, knee = a
#na = (hip, thigh, knee + numpy.radians(4))
#verify("new: %s" % (na, ))
if args.force:
def verify(msg):
print(msg)
#write_angles(conn, *na)
#sys.exit(0)
# set starting point
start_angles = numpy.array((hip, thigh, knee))
start_point = numpy.array(leg.forward(*start_angles))
verify("Starting angles: %s" % (numpy.degrees(start_angles), ))
verify("Starting angles: %s" % (start_angles, ))
verify("Starting point: %s" % (start_point, ))
end_point = start_point + map(float, args.end.strip('\\').split(','))
#end_point = start_point + (0.1, 0., 0.)
verify("End point: %s" % (end_point, ))
# set ending point
end_angles = numpy.array(leg.inverse(*end_point))
# set max delta angle
delta_angles = end_angles - start_angles
verify("Delta angles: %s" % (numpy.degrees(delta_angles), ))
# compute number of moves
n_moves = int(numpy.floor(numpy.max(numpy.abs(delta_angles) / args.delta)))
verify("N moves: %s" % (n_moves, ))
angle_steps = delta_angles / n_moves
verify("Angle steps: %s" % (angle_steps, ))
# while loop
current_angles = start_angles
current_target = start_angles
verify("Start?")
# send target
write_angles(conn, *current_target)
i = 0
while i < n_moves:
# try to read angles
a = read_angles(conn)
if a is not None:
# parse new angles
current_angles = numpy.array(a)
error = numpy.abs(current_angles - current_target)
if numpy.max(error) < args.close:
# target hit, move to next
i += 1
if i == n_moves:
break
current_target = start_angles + i * angle_steps
# send target
write_angles(conn, *current_target)
else:
# wait...
time.sleep(0.01)
def main():
p = argparse.ArgumentParser()
p.add_argument('-x', '--x', default=0., type=float)
p.add_argument('-y', '--y', default=0., type=float)
p.add_argument('-z', '--z', default=0., type=float)
p.add_argument('-X', '--dx', default=0., type=float)
p.add_argument('-Y', '--dy', default=0., type=float)
p.add_argument('-Z', '--dz', default=0., type=float)
p.add_argument('-r', '--raw', default=False, action='store_true')
args = p.parse_args()
# xy looking down
x = args.x
y = args.y
z = args.z
compute = not args.raw
lc = lambda joint: '/stompyleg/%s_controller/command' % (joint, )
fm = lambda data: std_msgs.msg.Float64(data)
if compute:
hip_angle, thigh_angle, knee_angle = leg.inverse(x, y, z)
else:
hip_angle = x
thigh_angle = y
knee_angle = z
rospy.init_node('stompyleg_move', anonymous=True)
hp = rospy.Publisher(lc('hip'), std_msgs.msg.Float64, queue_size=18)
tp = rospy.Publisher(lc('thigh'), std_msgs.msg.Float64, queue_size=18)
kp = rospy.Publisher(lc('knee'), std_msgs.msg.Float64, queue_size=18)
dx = args.dx
dy = args.dy
dz = args.dz
while not rospy.is_shutdown():
print(
"Hip: %s, Thigh: %s, Knee: %s" % (
hip_angle, thigh_angle, knee_angle))
hp.publish(hip_angle)
tp.publish(thigh_angle)
kp.publish(knee_angle)
x += dx
y += dy
z += dz
if compute:
hip_angle, thigh_angle, knee_angle = leg.inverse(x, y, z)
else:
hip_angle = x
thigh_angle = y
knee_angle = z
at_limit = False
if (hip_angle < leg.hip_limits[0] or hip_angle > leg.hip_limits[1]):
print("Hit hip limit")
at_limit = True
if (
thigh_angle < leg.thigh_limits[0] or
thigh_angle > leg.thigh_limits[1]):
print("Hit thigh limit")
at_limit = True
if (
knee_angle < leg.knee_limits[0] or
knee_angle > leg.knee_limits[1]):
print("Hit knee limit")
at_limit = True
if at_limit:
dx *= -1
dy *= -1
dz *= -1
x += dx
y += dy
z += dz
rospy.sleep(0.1)
def main():
p = argparse.ArgumentParser()
p.add_argument('-z', '--sz', default=0.5, type=float)
p.add_argument('-Z', '--ez', default=0.8, type=float)
p.add_argument('-x', '--x', default=1.5, type=float)
p.add_argument('-t', '--time', default=1., type=float)
p.add_argument('-r', '--rate', default=10., type=float)
args = p.parse_args()
# subscribe to joint messages
rospy.init_node('stompy_stand', anonymous=True)
rospy.Subscriber("/stompy/joint_states",
sensor_msgs.msg.JointState,
lambda data, description=joints.stompy_leg_descriptions:
joints.update_joints(data, description))
# setup publishers for commanding joints
leg_publishers = {}
foot_paths = {}
#for leg_name in ('fl', 'fr', 'ml', 'mr', 'rl', 'rr'):
for leg_name in ('fl', 'fr', 'ml', 'mr', 'rl', 'rr'):
#leg_publishers[leg_name] = rospy.Publisher(
# lc(leg_name), trajectory_msgs.msg.JointTrajectory, queue_size=10)
leg_publishers[leg_name] = actionlib.SimpleActionClient(
'/stompy/%s/follow_joint_trajectory' % leg_name,
control_msgs.msg.FollowJointTrajectoryAction)
foot_paths[leg_name] = PathTracer(time=args.time, rate=args.rate)
foot_paths[leg_name].skip = False
leg_publishers.values()[0].wait_for_server()
state = 0 # awaiting joints
tx = 0.25
ty = 0.25
tz = 0.25
ax = 10
ay = 10
az = 10
moves = [
# stand
(2, args.sz, 0),
(0, args.x, 0),
(2, args.ez, 0),
# leg lifts
(0, args.sz, 4),
(0, args.ez, 4),
(1, args.sz, 4),
(1, args.ez, 4),
(2, args.sz, 4),
(2, args.ez, 4),
(3, args.sz, 4),
(3, args.ez, 4),
(4, args.sz, 4),
(4, args.ez, 4),
(5, args.sz, 4),
(5, args.ez, 4),
# body translations
(0, tx, 3),
(0, -tx * 2, 3),
(0, tx, 3),
(1, ty, 3),
(1, -ty * 2, 3),
(1, ty, 3),
(2, tz, 3),
(2, -tz * 2, 3),
(2, tz, 3),
# body rotations
(0, ax, 5),
(0, -ax * 2, 5),
(0, ax, 5),
(1, ay, 5),
(1, -ay * 2, 5),
(1, ay, 5),
(2, az, 5),
(2, -az * 2, 5),
(2, az, 5),
]
sleep_time = 1. / args.rate
print("Waiting for connection...")
rospy.sleep(1.)
while not rospy.is_shutdown():
if state == 0: # awaiting joints
if joints.joints is not None:
# setup paths using computed start points
target_axis, target, new_state = moves.pop(0)
if new_state != state:
state = new_state
moves.insert(0, (target_axis, target, new_state))
continue
for leg_name in joints.legs:
# compute foot position
foot = leg.forward(joints.legs[leg_name]['hip'],
joints.legs[leg_name]['thigh'],
joints.legs[leg_name]['knee'])
# setup path for foot
foot_paths[leg_name].start = foot
ep = [foot[0], 0., foot[2]]
ep[target_axis] = target
h, t, k = leg.inverse(*ep)
if not any((
leg.in_limits(h, leg.hip_limits),
leg.in_limits(t, leg.thigh_limits),
leg.in_limits(k, leg.knee_limits),
)):
raise Exception()
foot_paths[leg_name].end = ep
# compute distance, set time
distance = numpy.linalg.norm(
numpy.array(ep) -
numpy.array(foot_paths[leg_name].start))
# limit speed to 0.01 m/s
t = max(1., distance / 0.75)
foot_paths[leg_name].time = t
print("Preparing %s foot move from %s to %s" %
(leg_name, foot, ep))
state = 1 # moving feet
else:
print("Awaiting joints")
elif state == 1: # moving feet
start_time = rospy.Time.now() + rospy.Duration(0.5)
# convert paths to trajectories
for leg_name in foot_paths:
if foot_paths[leg_name].skip:
continue
t = path_to_trajectory(foot_paths[leg_name], leg_name,
start_time)
leg_publishers[leg_name].send_goal(t)
leg_publishers[leg_name].last_goal = t
state = 2 # go to 'wait' state
elif state == 2: # wait
dones = []
for leg_name in foot_paths:
if foot_paths[leg_name].skip:
continue
p = leg_publishers[leg_name]
s = p.get_state()
dones.append(s == 3)
if s == 3:
foot_paths[leg_name] = PathTracer(time=args.time,
rate=args.rate)
foot_paths[leg_name].skip = False
elif s == 1:
pass
#ns = p.action_client.ns
#lp = p.last_goal.trajectory.points[-1]
#cp = numpy.array([
# joints.legs[leg_name][j] for j in
# ('hip', 'thigh', 'calf')])
#print leg_name, numpy.array(lp.positions) - cp
elif s != 1:
print s, p.get_result()
t = rospy.Time.now()
ns = p.action_client.ns
lp = p.last_goal.trajectory.points[-1]
tt = (lp.time_from_start +
p.last_goal.trajectory.header.stamp)
leg_name = ns.split('/')[2]
ap = joints.legs[leg_name].copy()
print(leg_name)
print(tt)
print(lp)
print(ap)
raise Exception
if all(dones):
print("Done moving")
if len(moves):
state = 0
else:
break
elif state == 3: # body shift
target_axis, target, new_state = moves.pop(0)
if new_state != state:
state = new_state
moves.insert(0, (target_axis, target, new_state))
continue
for leg_name in joints.legs:
# compute foot position
foot = leg.forward(joints.legs[leg_name]['hip'],
joints.legs[leg_name]['thigh'],
joints.legs[leg_name]['knee'])
# setup path for foot
foot_paths[leg_name].start = foot
ep = list(body.leg_to_body(leg_name, *foot))
ep[target_axis] += target
ep = body.body_to_leg(leg_name, *ep)
h, t, k = leg.inverse(*ep)
if not any((
leg.in_limits(h, leg.hip_limits),
leg.in_limits(t, leg.thigh_limits),
leg.in_limits(k, leg.knee_limits),
)):
raise Exception()
foot_paths[leg_name].end = ep
# compute distance, set time
#distance = numpy.linalg.norm(
# numpy.array(ep) -
# numpy.array(foot_paths[leg_name].start))
## limit speed to 0.01 m/s
#t = max(1., distance / 0.1)
foot_paths[leg_name].time = args.time
print("Preparing %s foot move from %s to %s" %
(leg_name, foot, ep))
state = 1
elif state == 4: # leg lifts
target_axis, target, new_state = moves.pop(0)
if new_state != state:
state = new_state
moves.insert(0, (target_axis, target, new_state))
continue
for (i, leg_name) in enumerate(
('fl', 'fr', 'ml', 'mr', 'rl', 'rr')):
if i != target_axis:
foot_paths[leg_name].skip = True
continue
foot_paths[leg_name].skip = False
# compute foot position
foot = leg.forward(joints.legs[leg_name]['hip'],
joints.legs[leg_name]['thigh'],
joints.legs[leg_name]['knee'])
# setup path for foot
foot_paths[leg_name].start = foot
ep = [foot[0], foot[1], target]
h, t, k = leg.inverse(*ep)
if not any((
leg.in_limits(h, leg.hip_limits),
leg.in_limits(t, leg.thigh_limits),
leg.in_limits(k, leg.knee_limits),
)):
raise Exception()
foot_paths[leg_name].end = ep
# compute distance, set time
distance = numpy.linalg.norm(
numpy.array(ep) - numpy.array(foot_paths[leg_name].start))
# limit speed to 0.01 m/s
t = max(1., distance / 0.75)
foot_paths[leg_name].time = t
print("Preparing %s foot move from %s to %s" %
(leg_name, foot, ep))
state = 1
elif state == 5: # body rotations
target_axis, target, new_state = moves.pop(0)
if new_state != state:
state = new_state
moves.insert(0, (target_axis, target, new_state))
continue
angles = [0, 0, 0]
angles[target_axis] = target
rm = transforms.rotation_3d(angles[0],
angles[1],
angles[2],
degrees=True)
for leg_name in joints.legs:
# compute foot position
foot = leg.forward(joints.legs[leg_name]['hip'],
joints.legs[leg_name]['thigh'],
joints.legs[leg_name]['knee'])
# setup path for foot
foot_paths[leg_name].start = foot
ep = list(body.leg_to_body(leg_name, *foot))
# TODO make these not 'straight' lines
ep = transforms.transform_3d(rm, *ep)
ep = body.body_to_leg(leg_name, *ep)
h, t, k = leg.inverse(*ep)
if not any((
leg.in_limits(h, leg.hip_limits),
leg.in_limits(t, leg.thigh_limits),
leg.in_limits(k, leg.knee_limits),
)):
raise Exception()
foot_paths[leg_name].end = ep
# compute distance, set time
#distance = numpy.linalg.norm(
# numpy.array(ep) -
# numpy.array(foot_paths[leg_name].start))
## limit speed to 0.01 m/s
#t = max(1., distance / 0.1)
foot_paths[leg_name].time = args.time
print("Preparing %s foot move from %s to %s" %
(leg_name, foot, ep))
state = 1
rospy.sleep(sleep_time)
def main():
p = argparse.ArgumentParser()
p.add_argument('-z', '--sz', default=0.5, type=float)
p.add_argument('-Z', '--ez', default=1.2, type=float)
p.add_argument('-x', '--x', default=1.5, type=float)
p.add_argument('-t', '--time', default=5., type=float)
p.add_argument('-r', '--rate', default=10., type=float)
args = p.parse_args()
# subscribe to joint messages
rospy.init_node('stompy_stand', anonymous=True)
rospy.Subscriber("/stompy/joint_states",
sensor_msgs.msg.JointState,
lambda data, description=joints.stompy_leg_descriptions:
joints.update_joints(data, description))
# setup publishers for commanding joints
lc = lambda leg, joint: '/stompy/%s_%s_controller/command' % (
leg,
joint,
)
leg_publishers = {}
foot_paths = {}
for leg_name in ('fl', 'fr', 'ml', 'mr', 'rl', 'rr'):
leg_publishers[leg_name] = {}
foot_paths[leg_name] = PathTracer(time=args.time, rate=args.rate)
for joint_name in ('hip', 'thigh', 'knee'):
leg_publishers[leg_name][joint_name] = rospy.Publisher(
lc(leg_name, joint_name), std_msgs.msg.Float64, queue_size=18)
state = 0 # awaiting joints
target_axis = 2
moves = 0
target = args.sz
sleep_time = 1. / args.rate
while not rospy.is_shutdown():
if state == 0: # awaiting joints
if joints.joints is not None:
# setup paths using computed start points
for leg_name in joints.legs:
# compute foot position
foot = leg.forward(joints.legs[leg_name]['hip'],
joints.legs[leg_name]['thigh'],
joints.legs[leg_name]['knee'])
# setup path for foot
foot_paths[leg_name].start = foot
ep = [foot[0], 0., foot[2]]
ep[target_axis] = target
h, t, k = leg.inverse(*ep)
if not any((
leg.in_limits(h, leg.hip_limits),
leg.in_limits(t, leg.thigh_limits),
leg.in_limits(k, leg.knee_limits),
)):
raise Exception()
foot_paths[leg_name].end = ep
print("Preparing %s foot move from %s to %s" %
(leg_name, foot, ep))
state = 1 # moving feet
else:
print("Awaiting joints")
elif state == 1: # moving feet
# execute paths
state = -1
for leg_name in foot_paths:
path = foot_paths[leg_name]
pub = leg_publishers[leg_name]
pt = path.next()
if pt is None:
continue
state = 1
h, t, k = leg.inverse(pt[0], pt[1], pt[2])
pub['hip'].publish(h)
pub['thigh'].publish(t)
pub['knee'].publish(k)
if state == -1:
for leg_name in ('fl', 'fr', 'ml', 'mr', 'rl', 'rr'):
foot_paths[leg_name] = PathTracer(time=args.time,
rate=args.rate)
if moves == 0:
target_axis = 0
target = args.x
state = 0
elif moves == 1:
target_axis = 2
target = args.ez
state = 0
# moves = 7 # skip to leg lifts
elif moves == 2:
# start body moves
target_axis = 0
target = 0.5
state = 2
elif moves == 3:
target_axis = 0
target = -1.0
state = 2
elif moves == 4:
target_axis = 0
target = 0.5
state = 2
elif moves == 5:
target_axis = 1
target = 0.5
state = 2
elif moves == 6:
target_axis = 1
target = -1.0
state = 2
elif moves == 7:
target_axis = 1
target = 0.5
state = 2
elif moves == 8: # leg lifts
target_axis = 'fl'
target = 0.2
state = 3
elif moves == 9:
target_axis = 'fl'
target = args.ez
state = 3
elif moves == 10:
target_axis = 'fr'
target = 0.2
state = 3
elif moves == 11:
target_axis = 'fr'
target = args.ez
state = 3
elif moves == 12:
target_axis = 'ml'
target = 0.2
state = 3
elif moves == 13:
target_axis = 'ml'
target = args.ez
state = 3
elif moves == 14:
target_axis = 'mr'
target = 0.2
state = 3
elif moves == 15:
target_axis = 'mr'
target = args.ez
state = 3
elif moves == 16:
target_axis = 'rl'
target = 0.2
state = 3
elif moves == 17:
target_axis = 'rl'
target = args.ez
state = 3
elif moves == 18:
target_axis = 'rr'
target = 0.2
state = 3
elif moves == 19:
target_axis = 'rr'
target = args.ez
state = 3
moves += 1
elif state == 2: # move body
if joints.joints is not None:
# setup paths using computed start points
for leg_name in joints.legs:
# compute foot position
foot = leg.forward(joints.legs[leg_name]['hip'],
joints.legs[leg_name]['thigh'],
joints.legs[leg_name]['knee'])
# compute in body coordinates
p = list(body.leg_to_body(leg_name, *foot))
p[target_axis] += target
f = body.body_to_leg(leg_name, *p)
# setup path for foot
foot_paths[leg_name].start = foot
foot_paths[leg_name].end = f
print("Preparing %s foot move from %s to %s" %
(leg_name, foot, ep))
state = 1 # moving feet
else:
print("Awaiting joints")
elif state == 3: # lift leg
if joints.joints is not None:
# setup paths using computed start points
for leg_name in joints.legs:
# compute foot position
foot = leg.forward(joints.legs[leg_name]['hip'],
joints.legs[leg_name]['thigh'],
joints.legs[leg_name]['knee'])
foot_paths[leg_name].start = foot
if leg_name == target_axis:
ep = [foot[0], foot[1], target]
else:
ep = foot
h, t, k = leg.inverse(*ep)
if not any((
leg.in_limits(h, leg.hip_limits),
leg.in_limits(t, leg.thigh_limits),
leg.in_limits(k, leg.knee_limits),
)):
raise Exception()
foot_paths[leg_name].end = ep
print("Preparing %s foot move from %s to %s" %
(leg_name, foot, ep))
state = 1 # moving feet
else:
print("Awaiting joints")
else:
print("Done moving")
break
rospy.sleep(sleep_time)