def run(self, integration_step=0.02, max_iterations=200):
integrator = CommandIntegrator(self.qp_type(self.hard_constraints, self.soft_constraints, self.controlled_values), self.int_rules, self.start_state, self.recorded_terms)
integrator.restart(self.name)
integrator.run(integration_step, max_iterations)
self.value_recorder = integrator.recorder
self.symbol_recorder = integrator.sym_recorder
from kineverse.motion.integrator import CommandIntegrator
from kineverse.utils import res_pkg_path
if __name__ == '__main__':
eef = point3(1,0,0)
goal = rotation3_rpy(0, -0.8, 2.56) * point3(1, 0, 0)
rpy_model = rotation3_rpy(Position('roll'), Position('pitch'), Position('yaw')) * eef
axis = vector3(Position('x'), Position('y'), 0) #Position('z'))
ax_model = rotation3_axis_angle(axis / (norm(axis) + 1e-4), norm(axis)) * eef
goal_rpy = SC(-norm(goal - rpy_model), -norm(goal - rpy_model), 1, norm(goal - rpy_model))
goal_ax = SC(-norm(goal - ax_model), -norm(goal - ax_model), 1, norm(goal - ax_model))
rpy_integrator = CommandIntegrator(TQPB({}, {'rpy': goal_rpy},
{str(s): CV(-0.6, 0.6, get_diff_symbol(s), 0.001) for s in rpy_model.free_symbols}),
recorded_terms={'dist rpy': norm(goal - rpy_model)})
ax_integrator = CommandIntegrator(TQPB({}, {'ax': goal_ax},
{str(s): CV(-0.6, 0.6, get_diff_symbol(s), 0.001) for s in ax_model.free_symbols}),
recorded_terms={'dist ax': norm(goal - ax_model)})
rpy_integrator.restart('Convergence Test: RPY vs AxAngle')
rpy_integrator.run()
ax_integrator.restart('Convergence Test: RPY vs AxAngle')
ax_integrator.run()
draw_recorders([rpy_integrator.recorder, ax_integrator.recorder,
rpy_integrator.sym_recorder, ax_integrator.sym_recorder], 1, 4, 4).savefig('{}/rpy_vs_axis_angle.png'.format(res_pkg_path('package://kineverse_experiment_world/test')))
get_diff(pos_of(base_link.to_parent)[2].subs({roomba_joint.z_pos: 0})),
roomba_joint.a_pos:
roomba_joint.a_pos + DT_SYM * roomba_joint.ang_vel
},
recorded_terms={
'distance': dist.expr,
'gaze_align': look_goal.expr,
'location_x': roomba_joint.x_pos,
'location_y': roomba_joint.y_pos,
'location_z': roomba_joint.z_pos,
'rotation_a': roomba_joint.a_pos
})
# RUN
int_factor = 0.02
integrator.restart('Fetch Cartesian Goal Example')
integrator.run(int_factor)
# DRAW
draw_recorders([integrator.recorder] +
split_recorders([integrator.sym_recorder]), 4.0 / 9.0, 8,
4).savefig('{}/fetch_sandbox_plots.png'.format(plot_dir))
trajectory = {
Path(j)[-1][:-2]: [0.0] * len(integrator.recorder.data.values()[0])
for j in joint_symbols
}
trajectory.update({
Path(se.Symbol(j))[-1][:-2]: d
for j, d in integrator.recorder.data.items()
})
recorded_terms={
'distance':
pushing_controller.geom_distance,
'gaze_align':
pushing_controller.look_goal,
'in contact':
pushing_controller.in_contact,
'location_x': base_joint.x_pos,
'location_y': base_joint.y_pos,
'rotation_a': base_joint.a_pos
},
printed_exprs={})
# RUN
int_factor = 0.02
integrator.restart(f'{robot} Cartesian Goal Example')
# print('\n'.join('{}: {}'.format(s, r) for s, r in integrator.integration_rules.items()))
try:
start = Time.now()
integrator.run(int_factor,
1200,
logging=False,
real_time=False,
show_progress=True)
total_dur.append((Time.now() - start).to_sec())
n_iter.append(integrator.current_iteration + 1)
except Exception as e:
print(f'Exception during integration:\n{e}')
# DRAW
print('Drawing recorders')
class TrackerNode(object):
def __init__(self,
js_topic,
obs_topic,
integration_factor=0.05,
iterations=30,
visualize=False,
use_timer=True):
self.km_client = GeometryModel() # ModelClient(GeometryModel)
self._js_msg = ValueMapMsg()
self._integration_factor = integration_factor
self._iterations = iterations
self._use_timer = use_timer
self._unintialized_poses = set()
self.aliases = {}
self.tracked_poses = {}
self.integrator = None
self.lock = RLock()
self.soft_constraints = {}
self.joints = set()
self.joint_aliases = {}
self.visualizer = ROSVisualizer('/tracker_vis',
'world') if visualize else None
self.pub_js = rospy.Publisher(js_topic, ValueMapMsg, queue_size=1)
self.sub_obs = rospy.Subscriber(obs_topic,
PSAMsg,
self.cb_process_obs,
queue_size=5)
def track(self, model_path, alias):
with self.lock:
if type(model_path) is not str:
model_path = str(model_path)
start_tick = len(self.tracked_poses) == 0 and self._use_timer
if model_path not in self.tracked_poses:
syms = [
Position(Path(model_path) + (x, ))
for x in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']
]
def rf(msg, state):
state[syms[0]] = msg.position.x
state[syms[1]] = msg.position.y
state[syms[2]] = msg.position.z
state[syms[3]] = msg.orientation.x
state[syms[4]] = msg.orientation.y
state[syms[5]] = msg.orientation.z
state[syms[6]] = msg.orientation.w
def process_model_update(data):
self._generate_pose_constraints(model_path, data)
axis = vector3(*syms[:3])
self.aliases[alias] = model_path
self.tracked_poses[model_path] = TrackerEntry(
frame3_quaternion(*syms), rf, process_model_update)
self._unintialized_poses.add(model_path)
if type(self.km_client) == ModelClient:
self.km_client.register_on_model_changed(
Path(model_path), process_model_update)
else:
process_model_update(self.km_client.get_data(model_path))
if start_tick:
self.timer = rospy.Timer(rospy.Duration(1.0 / 50),
self.cb_tick)
def stop_tracking(self, model_path):
with self.lock:
if type(model_path) is not str:
model_path = str(model_path)
if model_path in self.tracked_poses:
te = self.tracked_poses[model_path]
self.km_client.deregister_on_model_changed(te.model_cb)
del self.tracked_poses[model_path]
del self.soft_constraints['{} align rotation 0'.format(
model_path)]
del self.soft_constraints['{} align rotation 1'.format(
model_path)]
del self.soft_constraints['{} align rotation 2'.format(
model_path)]
del self.soft_constraints['{} align position'.format(
model_path)]
self.generate_opt_problem()
if len(self.tracked_poses) == 0:
self.timer.shutdown()
@profile
def cb_tick(self, timer_event):
with self.lock:
if self.integrator is not None:
self.integrator.run(self._integration_factor,
self._iterations,
logging=False)
self._js_msg.header.stamp = Time.now()
self._js_msg.symbol, self._js_msg.value = zip(
*[(n, self.integrator.state[s])
for s, n in self.joint_aliases.items()])
self.pub_js.publish(self._js_msg)
if self.visualizer is not None:
poses = [
t.pose.subs(self.integrator.state)
for t in self.tracked_poses.values()
]
self.visualizer.begin_draw_cycle('obs_points')
self.visualizer.draw_points(
'obs_points', cm.eye(4), 0.1,
[pos_of(p) for p in poses if len(p.free_symbols) == 0])
# self.visualizer.draw_poses('obs_points', se.eye(4), 0.1, 0.02, [p for p in poses if len(p.free_symbols) == 0])
self.visualizer.render('obs_points')
else:
print('Integrator does not exist')
@profile
def cb_process_obs(self, poses_msg):
# print('Got new observation')
with self.lock:
for pose_msg in poses_msg.poses:
if pose_msg.header.frame_id in self.aliases and self.integrator is not None:
self.tracked_poses[self.aliases[
pose_msg.header.frame_id]].update_state(
pose_msg.pose, self.integrator.state)
if self.aliases[pose_msg.header.
frame_id] in self._unintialized_poses:
self._unintialized_poses.remove(
self.aliases[pose_msg.header.frame_id])
def _generate_pose_constraints(self, str_path, model):
with self.lock:
if str_path in self.tracked_poses:
align_rotation = '{} align rotation'.format(str_path)
align_position = '{} align position'.format(str_path)
if model is not None:
m_free_symbols = cm.free_symbols(model.pose)
if len(m_free_symbols) > 0:
te = self.tracked_poses[str_path]
self.joints |= m_free_symbols
self.joint_aliases = {s: str(s) for s in self.joints}
r_dist = norm(
cm.rot_of(model.pose) - cm.rot_of(te.pose))
self.soft_constraints[align_rotation] = SC(
-r_dist, -r_dist, 1, r_dist)
# print(align_position)
# print(model.pose)
dist = norm(cm.pos_of(model.pose) - cm.pos_of(te.pose))
# print('Distance expression:\n{}'.format(dist))
# print('Distance expression symbol overlap:\n{}'.format(m_free_symbols.intersection(cm.free_symbols(dist))))
# for s in m_free_symbols:
# print('Diff w.r.t {}:\n{}'.format(s, cm.diff(dist, s)))
self.soft_constraints[align_position] = SC(
-dist, -dist, 1, dist)
self.generate_opt_problem()
# Avoid crashes due to insufficient perception data. This is not fully correct.
if str_path in self._unintialized_poses:
state = self.integrator.state.copy(
) if self.integrator is not None else {}
state.update({
s: 0.0
for s in m_free_symbols if s not in state
})
null_pose = cm.subs(model.pose, state)
pos = cm.pos_of(null_pose)
quat = real_quat_from_matrix(cm.rot_of(null_pose))
msg = PoseMsg()
msg.position.x = pos[0]
msg.position.y = pos[1]
msg.position.z = pos[2]
msg.orientation.x = quat[0]
msg.orientation.y = quat[1]
msg.orientation.z = quat[2]
msg.orientation.w = quat[3]
te.update_state(msg, self.integrator.state)
else:
regenerate_problem = False
if align_position in self.soft_constraints:
del self.soft_constraints[align_position]
regenerate_problem = True
if align_rotation in self.soft_constraints:
del self.soft_constraints[align_rotation]
regenerate_problem = True
if regenerate_problem:
self.generate_opt_problem()
def generate_opt_problem(self):
joint_symbols = self.joints
opt_symbols = {DiffSymbol(j) for j in joint_symbols}
hard_constraints = self.km_client.get_constraints_by_symbols(
joint_symbols | opt_symbols)
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, opt_symbols)
for mp in self._unintialized_poses:
state = self.integrator.state if self.integrator is not None else {}
te = self.tracked_poses[mp]
state.update({s: 0.0 for s in cm.free_symbols(te.pose)})
state = {} if self.integrator is None else self.integrator.state
self.integrator = CommandIntegrator(TQPB(hard_constraints,
self.soft_constraints,
controlled_values),
start_state=state)
self.integrator.restart('Pose Tracking')
for x, r_goal in enumerate(random_rot_uniform(r_points)):
goal_ax, goal_a = axis_angle_from_matrix(r_goal)
goal_axa = goal_ax * goal_a
t_results = {}
for k, m in methods.items():
r_ctrl = rs[k]
constraints = m(r_ctrl, r_goal)
integrator = CommandIntegrator(TQPB({},
constraints,
{str(s): CV(-1, 1, DiffSymbol(s), 1e-3) for s in sum([list(free_symbols(c.expr))
for c in constraints.values()], [])}),
start_state={s_x: 1, s_y: 0, s_z: 0}, equilibrium=0.001)#,
# recorded_terms=recorded_terms)
integrator.restart('Convergence Test {} {}'.format(x, k))
# try:
t_start = Time.now()
integrator.run(step, 100)
t_per_it = (Time.now() - t_start).to_sec() / integrator.current_iteration
final_r = rs_eval[k](**{str(s): v for s, v in integrator.state.items()})
final_ax, final_a = axis_angle_from_matrix(final_r)
final_axa = final_ax * final_a
error_9d = sum([np.abs(float(x)) for x in (final_r - r_goal).elements()])
error_axa_sym = norm(final_axa - goal_axa)
# print(final_ax, final_a, type(final_a))
error_axa = float(error_axa_sym)
t_results[k] = OptResult(error_9d,
error_axa,
integrator.recorder.data[str(s_x)],
integrator.recorder.data[str(s_y)],
is_unlocked = gm.alg_and(gm.greater_than(door_position, 0.4),
gm.less_than(handle_position, 0.15))
integrator = CommandIntegrator(
qp,
start_state=q_ik_goal,
recorded_terms={
'is_unlocked': is_unlocked,
'handle_position': handle_position,
'door_position': door_position
},
printed_exprs={'is_unlocked': is_unlocked})
try:
integrator.restart(title='Door opening generator')
integrator.run(dt=0.05,
max_iterations=500,
logging=True,
show_progress=False,
real_time=True)
draw_recorders([integrator.sym_recorder], 2, 8, 4).savefig(
res_pkg_path(
'package://kineverse_experiment_world/plots/door_opening_terms.png'
))
if PANDA_LOGGING:
rec_w, rec_b, rec_c, recs = convert_qp_builder_log(
integrator.qp_builder)
draw_recorders([rec_b, rec_c] + [
controlled_symbols.union({button_position}))
print('----\n{}'.format('\n'.join([str(x) for x in constraints.values()])))
cvs, constraints = generate_controlled_values(constraints,
controlled_symbols)
print('Retrieved Constraints:\n{}'.format('\n'.join(
['{}:\n {}'.format(k, c) for k, c in constraints.items()])))
integrator = CommandIntegrator(TQPB(
constraints, {'goal': SC(-0.1, -0.1, 1, button_position)}, cvs),
start_state={button_position: 0},
recorded_terms={
'lock_lower': lock_constraint.lower,
'lock_upper': lock_constraint.upper,
'spring_lower': spring_constraint.lower,
})
integrator.restart('Door spring')
integrator.run(dt=0.05)
integrator.recorder.add_threshold('Door release', point_of_release, 'b')
plot_dir = res_pkg_path('package://kineverse/test/plots')
draw_recorders([integrator.recorder] +
split_recorders([integrator.sym_recorder]), 4.0 / 9.0, 8,
4).savefig('{}/microwave_spring.png'.format(plot_dir))
traj_vis.visualize(integrator.recorder.data)
traj_vis.shutdown()
#integrator = CommandIntegrator(TQPB(constraints, goal_constraints, controlled_values),
integration_rules,
start_state=start_state,
recorded_terms={
'distance': geom_distance,
'gaze_align': look_goal,
'in contact': in_contact,
'goal': goal_constraints.values()[0].expr,
'location_x': base_joint.x_pos,
'location_y': base_joint.y_pos,
'rotation_a': base_joint.a_pos
})
# RUN
int_factor = 0.1
integrator.restart('{} Cartesian Goal Example'.format(robot))
print('\n'.join([
'{}: {}'.format(k, v)
for k, v in integrator.integration_rules.items()
]))
#exit(0)
try:
integrator.run(int_factor, 500)
except Exception as e:
print(e)
# DRAW
print('Drawing recorders')
draw_recorders([
filter_contact_symbols(integrator.recorder, integrator.qp_builder),
integrator.sym_recorder