def condition_controller(condition):
#from itertools import takewhile
dt = get_time_step()
for step in irange(INF):
#duration = step*dt
if condition(step):
break
yield
def follow_curve(robot,
joints,
curve,
goal_t=None,
time_step=None,
max_velocities=MAX_VELOCITIES,
**kwargs):
if goal_t is None:
goal_t = curve.x[-1]
if time_step is None:
time_step = 10 * get_time_step()
#distance_fn = get_distance_fn(robot, joints, weights=None, norm=2)
distance_fn = get_duration_fn(robot,
joints,
velocities=max_velocities,
norm=INF) # get_distance
positions = np.array(get_joint_positions(robot, joints))
closest_dist, closest_t = find_closest(positions,
curve,
t_range=(curve.x[0], goal_t),
max_time=1e-1,
max_iterations=INF,
distance_fn=distance_fn,
verbose=True)
print('Closest dist: {:.3f} | Closest time: {:.3f}'.format(
closest_dist, closest_t))
target_t = closest_t
# TODO: condition based on closest_dist
while True:
print('\nTarget time: {:.3f} | Goal time: {}'.format(target_t, goal_t))
target_positions = curve(target_t)
target_velocities = curve(target_t, nu=1) # TODO: draw the velocity
#print('Positions: {} | Velocities: {}'.format(target_positions, target_velocities))
handles = draw_waypoint(target_positions)
is_goal = (target_t == goal_t)
position_tol = 1e-2 if is_goal else 1e-2
for output in control_state(robot,
joints,
target_positions=target_positions,
target_velocities=target_velocities,
position_tol=position_tol,
velocity_tol=INF,
max_velocities=max_velocities,
**kwargs):
yield output
remove_handles(handles)
target_t = min(goal_t, target_t + time_step)
if is_goal:
break
def rest_for_duration(self, duration):
if not self.execute_motor_control:
return
sim_duration = 0.0
body = self.robot
position_gain = 0.02
with ClientSaver(self.client):
# TODO: apply to all joints
dt = get_time_step()
movable_joints = get_movable_joints(body)
target_conf = get_joint_positions(body, movable_joints)
while sim_duration < duration:
p.setJointMotorControlArray(
body,
movable_joints,
p.POSITION_CONTROL,
targetPositions=target_conf,
targetVelocities=[0.0] * len(movable_joints),
positionGains=[position_gain] * len(movable_joints),
# velocityGains=[velocity_gain] * len(movable_joints),
physicsClientId=self.client)
step_simulation()
sim_duration += dt
def simulate(controller=None,
max_duration=INF,
max_steps=INF,
print_rate=1.,
sleep=None):
enable_gravity()
dt = get_time_step()
print('Time step: {:.6f} sec'.format(dt))
start_time = last_print = time.time()
for step in irange(max_steps):
duration = step * dt
if duration >= max_duration:
break
if (controller is not None) and is_empty(controller):
break
step_simulation()
synchronize_viewer()
if elapsed_time(last_print) >= print_rate:
print(
'Sim step: {} | Sim time: {:.3f} sec | Elapsed time: {:.3f} sec'
.format(step, duration, elapsed_time(start_time)))
last_print = time.time()
if sleep is not None:
time.sleep(sleep)
def follow_trajectory(self,
joints,
path,
times_from_start=None,
real_time_step=0.0,
waypoint_tolerance=1e-2 * np.pi,
goal_tolerance=5e-3 * np.pi,
max_sim_duration=1.0,
print_sim_frequency=1.0,
**kwargs):
# real_time_step = 1e-1 # Can optionally sleep to slow down visualization
start_time = time.time()
if times_from_start is not None:
assert len(path) == len(times_from_start)
current_positions = get_joint_positions(self.robot, joints)
differences = [(np.array(q2) - np.array(q1)) / (t2 - t1)
for q1, t1, q2, t2 in
zip([current_positions] + path, [0.] +
times_from_start, path, times_from_start)]
velocity_path = differences[1:] + [np.zeros(len(joints))
] # Using velocity at endpoints
else:
velocity_path = [None] * len(path)
sim_duration = 0.0
sim_steps = 0
last_print_sim_duration = sim_duration
with ClientSaver(self.client):
dt = get_time_step()
# TODO: fit using splines to get velocity info
for num, positions in enumerate(path):
if self.execute_motor_control:
sim_start = sim_duration
tolerance = goal_tolerance if (num == len(path) -
1) else waypoint_tolerance
velocities = velocity_path[num]
for _ in joint_controller_hold2(self.robot,
joints,
positions,
velocities,
tolerance=tolerance,
**kwargs):
step_simulation()
# print(get_joint_velocities(self.robot, joints))
# print([get_joint_torque(self.robot, joint) for joint in joints])
sim_duration += dt
sim_steps += 1
time.sleep(real_time_step)
if print_sim_frequency <= (sim_duration -
last_print_sim_duration):
print(
'Waypoint: {} | Simulation steps: {} | Simulation seconds: {:.3f} | Steps/sec {:.3f}'
.format(num, sim_steps, sim_duration,
sim_steps / elapsed_time(start_time)))
last_print_sim_duration = sim_duration
if max_sim_duration <= (sim_duration - sim_start):
print(
'Timeout of {:.3f} simulation seconds exceeded!'
.format(max_sim_duration))
break
# control_joints(self.robot, arm_joints, positions)
else:
set_joint_positions(self.robot, joints, positions)
print(
'Followed {} waypoints in {:.3f} simulation seconds and {} simulation steps'
.format(len(path), sim_duration, sim_steps))
def follow_curve_old(robot, joints, curve, goal_t=None):
# TODO: unify with /Users/caelan/Programs/open-world-tamp/open_world/simulation/control.py
if goal_t is None:
goal_t = curve.x[-1]
time_step = get_time_step()
target_step = 10 * time_step
#distance_fn = get_distance_fn(robot, joints, weights=None, norm=2)
distance_fn = get_duration_fn(robot,
joints,
velocities=MAX_VELOCITIES,
norm=INF)
for i in irange(INF):
# if (i % 10) != 0:
# continue
current_p = np.array(get_joint_positions(robot, joints))
current_v = np.array(get_joint_velocities(robot, joints))
goal_dist = distance_fn(current_p, curve(goal_t))
print('Positions: {} | Velocities: {} | Goal distance: {:.3f}'.format(
current_p.round(N_DIGITS), current_v.round(N_DIGITS), goal_dist))
if goal_dist < 1e-2:
return True
# _, connection = mpc(current_p, current_v, curve, v_max=MAX_VELOCITIES, a_max=MAX_ACCELERATIONS,
# dt_max=1e-1, max_time=1e-1)
# assert connection is not None
# target_t = 0.5*connection.x[-1]
# target_p = connection(target_t)
# target_v = connection(target_t, nu=1)
# #print(target_p)
closest_dist, closest_t = find_closest(current_p,
curve,
t_range=None,
max_time=1e-2,
max_iterations=INF,
distance_fn=distance_fn,
verbose=True)
target_t = min(closest_t + target_step, curve.x[-1])
target_p = curve(target_t)
#target_v = curve(target_t, nu=1)
target_v = curve(closest_t, nu=1)
#target_v = MAX_VELOCITIES
#target_v = INF*np.zeros(len(joints))
handles = draw_waypoint(target_p)
#times, confs = time_discretize_curve(curve, verbose=False, resolution=resolutions) # max_velocities=v_max,
# set_joint_positions(robot, joints, target_p)
max_velocity_control_joints(robot,
joints,
positions=target_p,
velocities=target_v,
max_velocities=MAX_VELOCITIES)
#next_t = closest_t + time_step
#next_p = current_p + current_v*time_step
yield target_t
actual_p = np.array(get_joint_positions(robot, joints))
actual_v = np.array(get_joint_velocities(robot, joints))
next_p = current_p + actual_v * time_step
print('Predicted: {} | Actual: {}'.format(next_p.round(N_DIGITS),
actual_p.round(N_DIGITS)))
remove_handles(handles)