def __init__(self, km,
actuated_pose,
goal_pose,
controlled_symbols,
weight_override=None,
draw_fn=None):
eef = actuated_pose
goal_pos_error = gm.norm(gm.pos_of(eef) - gm.pos_of(goal_pose))
self.eef_rot = eef[:3, :3]
self.goal_rot = goal_pose[:3, :3]
self.rot_error_matrix = self.eef_rot - self.goal_rot
goal_rot_error = gm.norm(self.rot_error_matrix)
rot_align_scale = gm.exp(-3 * goal_rot_error)
goal_constraints = {'align position': SC(-goal_pos_error * rot_align_scale,
-goal_pos_error * rot_align_scale, 10, goal_pos_error),
'align rotation': SC(-goal_rot_error, -goal_rot_error, 1, goal_rot_error)}
self.goal_rot_error = goal_rot_error
constraints = km.get_constraints_by_symbols(gm.free_symbols(eef).union(controlled_symbols))
cvs, constraints = generate_controlled_values(constraints, controlled_symbols, weights=weight_override)
cvs = depth_weight_controlled_values(km, cvs, exp_factor=1.02)
self.draw_fn = draw_fn
self.qp = TQPB(constraints,
goal_constraints,
cvs)
def __init__(self, km, controlled_symbols, resting_pose, camera_path=None):
tucking_constraints = {}
if resting_pose is not None:
tucking_constraints = {
f'tuck {s}': SC(p - s, p - s, 1, s)
for s, p in resting_pose.items()
}
# print('Tuck state:\n {}\nTucking constraints:\n {}'.format('\n '.join(['{}: {}'.format(k, v) for k, v in self._resting_pose.items()]), '\n '.join(tucking_constraints.keys())))
# tucking_constraints.update(self.taxi_constraints)
self.use_camera = camera_path is not None
if camera_path is not None:
self._poi_pos = gm.Symbol('poi')
poi = gm.point3(1.5, 0.5, 0.0) + gm.vector3(
0, self._poi_pos * 2.0, 0)
camera = km.get_data(camera_path)
cam_to_poi = poi - gm.pos_of(camera.pose)
lookat_dot = 1 - gm.dot_product(gm.x_of(camera.pose),
cam_to_poi) / gm.norm(cam_to_poi)
tucking_constraints['sweeping gaze'] = SC(-lookat_dot * 5,
-lookat_dot * 5, 1,
lookat_dot)
symbols = set()
for c in tucking_constraints.values():
symbols |= gm.free_symbols(c.expr)
joint_symbols = {
s
for s in symbols if gm.get_symbol_type(s) != gm.TYPE_UNKNOWN
}
controlled_symbols = {gm.DiffSymbol(s) for s in joint_symbols}
hard_constraints = km.get_constraints_by_symbols(
symbols.union(controlled_symbols))
controlled_values, hard_constraints = generate_controlled_values(
hard_constraints, controlled_symbols)
controlled_values = depth_weight_controlled_values(
km, controlled_values)
self.qp = TQPB(hard_constraints, tucking_constraints,
controlled_values)
self._start = Time.now()
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')
y1: y1 + DT_SYM * (rw_v * sin(a1) * r * 0.5 + lw_v * sin(a1) * r * 0.5),
a1: a1 + DT_SYM * (rw_v * (r / L) + lw_v * (- r / L)),
# x2: x2 + DT_SYM * l_v * cos(a2),
# y2: y2 + DT_SYM * l_v * sin(a2),
# a2: a2 + DT_SYM * a_v
}
goal = point3(1, 1, 0)
diff_dist = norm(dot(diff_drive, point3(0.1, 0, 0)) - goal) #
# md_dist = norm(dot(my_drive, point3(0.1, 0, 0)) - goal) #
qpb = TQPB({},
{'goal diff_drive': SC(-diff_dist, -diff_dist, 1, diff_dist)},
# 'goal my_drive': SC(-md_dist, -md_dist, 1, md_dist)},
{str(rw_v): CV(-r_limit, r_limit, rw_v, 0.001),
str(lw_v): CV(-r_limit, r_limit, lw_v, 0.001),
# str(l_v): CV(-1, 1, l_v, 0.001),
# str(a_v): CV(-a_limit, a_limit, a_v, 0.001)
})
full_diff_points = []
md_points = []
cmds = [
{rw_v: -0.8, lw_v: -0.8},
{rw_v: -1.0, lw_v: -0.0},
{rw_v: -0.0, lw_v: -1.0},
{rw_v: 0.8, lw_v: 0.8},
{rw_v: 1.0, lw_v: 0.0},
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')))
constraints = {k: c for k, c in constraints.items() if k not in to_remove}
for s in controlled_symbols:
if str(s) not in controlled_values:
controlled_values[str(s)] = ControlledValue(-1e9, 1e9, s, 0.01)
print('\n'.join(controlled_values.keys()))
goal_constraints = {
'reach_point': SoftConstraint(-dist, -dist, 1, dist),
'look_at_eef': SoftConstraint(-look_goal, -look_goal, 1, look_goal)
}
base_link = km.get_data('fetch/links/base_link')
integrator = CommandIntegrator(TQPB(
constraints, goal_constraints, controlled_values
), {
roomba_joint.x_pos:
roomba_joint.x_pos + DT_SYM *
get_diff(pos_of(base_link.to_parent)[0].subs({roomba_joint.x_pos: 0})),
roomba_joint.y_pos:
roomba_joint.y_pos + DT_SYM *
get_diff(pos_of(base_link.to_parent)[1].subs({roomba_joint.y_pos: 0})),
roomba_joint.z_pos:
roomba_joint.z_pos + DT_SYM *
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,
a = vector3(s_x, s_y, s_z)
rs[k] = rotation3_axis_angle(a / (norm(a) + 1e-4), norm(a))
rs_eval[k] = cm.speed_up(rs[k], free_symbols(rs[k]))
results = []
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)
def __init__(self, km, observations, transition_rules=None, max_iterations=20, num_samples=7):
"""Sets up an EKF estimating the underlying state of a set of observations.
Args:
km (ArticulationModel): Articulation model to query for constraints
observations (dict): A dict of observations. Names are mapped to
any kind of symbolic expression/matrix
transition_rules (dict, optional): Maps symbols to their transition rule.
Rules will be generated automatically, if not provided here.
"""
state_vars = union([gm.free_symbols(o) for o in observations.values()])
self.num_samples = num_samples
self.ordered_vars, \
self.transition_fn, \
self.transition_args = generate_transition_function(QPStateModel.DT_SYM,
state_vars,
transition_rules)
self.command_vars = {s for s in self.transition_args
if s not in state_vars and str(s) != str(QPStateModel.DT_SYM)}
obs_constraints = {}
obs_switch_vars = {}
# State as column vector n * 1
self.switch_vars = {}
self._obs_state = {}
self.obs_vars = {}
self.takers = {}
flat_obs = []
for o_label, o in sorted(observations.items()):
if gm.is_symbolic(o):
obs_switch_var = gm.Symbol(f'{o_label}_observed')
self.switch_vars[o_label] = obs_switch_var
if o_label not in obs_constraints:
obs_constraints[o_label] = {}
if o_label not in self.obs_vars:
self.obs_vars[o_label] = []
if gm.is_matrix(o):
if type(o) == gm.GM:
components = zip(sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []), iter(o))
else:
components = zip(sum([[(y, x) for x in range(o.shape[1])]
for y in range(o.shape[0])], []), o.T.elements()) # Casadi iterates vertically
indices = []
for coords, c in components:
if gm.is_symbolic(c):
obs_symbol = gm.Symbol('{}_{}_{}'.format(o_label, *coords))
obs_error = gm.abs(obs_symbol - c)
constraint = SC(-obs_error - (1 - obs_switch_var) * 1e3,
-obs_error + (1 - obs_switch_var) * 1e3, 1, obs_error)
obs_constraints[o_label][f'{o_label}:{Path(obs_symbol)}'] = constraint
self.obs_vars[o_label].append(obs_symbol)
indices.append(coords[0] * o.shape[1] + coords[1])
if len(indices) > 0:
self.takers[o_label] = indices
else:
obs_symbol = gm.Symbol(f'{o_label}_value')
obs_error = gm.abs(obs_symbol - c)
constraint = SC(-obs_error - obs_switch_var * 1e9,
-obs_error + obs_switch_var * 1e9, 1, obs_error)
obs_constraints[o_label][f'{o_label}:{Path(obs_symbol)}'] = constraint
self.obs_vars[o_label].append(obs_symbol)
self.takers[o_label] = [0]
state_constraints = km.get_constraints_by_symbols(state_vars)
cvs, hard_constraints = generate_controlled_values(state_constraints,
{gm.DiffSymbol(s) for s in state_vars
if gm.get_symbol_type(s) != gm.TYPE_UNKNOWN})
flat_obs_constraints = dict(sum([list(oc.items()) for oc in obs_constraints.values()], []))
self.qp = TQPB(hard_constraints, flat_obs_constraints, cvs)
st_bound_vars, st_bounds, st_unbounded = static_var_bounds(km, state_vars)
self._state = {s: 0 for s in st_unbounded} # np.random.uniform(-1.0, 1.0) for s in st_unbounded}
for vb, (lb, ub) in zip(st_bound_vars, st_bounds):
self._state[vb] = np.random.uniform(lb, ub)
self._state_buffer = []
self._state.update({s: 0 for s in self.transition_args})
self._obs_state = {s: 0 for s in sum(self.obs_vars.values(), [])}
self._obs_count = 0
self._stamp_last_integration = None
self._max_iterations = 10
self._current_error = 1e9
print(lock_constraint)
goal = 1.57 - door_position
controlled_symbols = {get_diff_symbol(button_position), door_velocity}
constraints = km.get_constraints_by_symbols(
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,