def test_sin(self):
x, y = [Position(x) for x in 'xy']
baseline = se.sin(x + 5 * y)
gc_x = GC(x)
gc_y = GC(5 * y)
# Generate gradients
gc_x[DiffSymbol(x)]
gc_y[DiffSymbol(y)]
gc_xy = sin(gc_x + gc_y)
self.assertEqual(gc_xy.expr, baseline)
self.assertEqual(gc_xy[DiffSymbol(x)], baseline.diff(x))
self.assertEqual(gc_xy[DiffSymbol(y)], baseline.diff(y))
def testow(self):
x, y = [Position(x) for x in 'xy']
baseline = x**(4 * y)
gc_x = GC(x)
gc_y = GC(4 * y)
# Generate gradients
gc_x[DiffSymbol(x)]
gc_y[DiffSymbol(y)]
gc_xy = gc_x**gc_y
self.assertEqual(gc_xy.expr, baseline)
self.assertEqual(gc_xy[DiffSymbol(x)], baseline.diff(x))
self.assertEqual(gc_xy[DiffSymbol(y)], baseline.diff(y))
def __isub__(self, other):
if type(other) == GradientContainer:
self.expr += other.expr
for k, v in other.gradients.items():
if k in self.gradients:
self.gradients[k] -= v
else:
self.gradients[k] = v
self.free_diff_symbols |= other.free_diff_symbols
else:
self.expr -= other
if hasattr(other, 'free_symbols'):
for f in other.free_symbols:
if DiffSymbol(f) in self.gradients:
self.gradients[DiffSymbol(f)] -= self.expr.diff(f)
else:
self.free_diff_symbols.add(DiffSymbol(f))
self.free_symbols = self.expr.free_symbols if hasattr(
self.expr, 'free_symbols') else set()
return self
def __init__(self, expr, gradient_exprs=None):
"""Constructor. Stores given expression an optional gradient information.
:param expr: Core expression to be stored.
:type expr: int, float, GradientContainer, symengine type
:param gradient_exprs: Dictionary of custom derivatives. Mapping: symengine.Symbol -> Expression
:type gradient_exprs: dict
"""
if type(expr) == GradientContainer:
self.expr = expr.expr
self.gradients = expr.gradients.copy()
if gradient_exprs is not None:
self.gradients.update(gradient_exprs)
else:
self.expr = expr if type(
expr) is not GradientContainer else expr.expr
self.gradients = gradient_exprs if gradient_exprs is not None else {}
self.free_symbols = expr.free_symbols if hasattr(
expr, 'free_symbols') else set()
self.free_diff_symbols = {
DiffSymbol(s)
for s in self.free_symbols if DiffSymbol(s) not in self.gradients
}
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')
def __init__(self):
super(LockboxOpeningGenerator, self).__init__('Generated Opening')
self.start_state = {
self.lock_a_p: 0,
self.lock_b_p: 0,
self.lock_c_p: 0,
self.lock_d_p: 0,
self.lock_e_p: 0,
self.lock_f_p: 0, #} # ,
self.lock_a_v: 0,
self.lock_b_v: 0,
self.lock_c_v: 0,
self.lock_d_v: 0,
self.lock_e_v: 0,
self.lock_f_v: 0
}
self.soft_constraints = lock_explorer(
self.km, self.start_state, {
'open_a':
SoftConstraint(1.2 - self.lock_a_p, 1.2 - self.lock_a_p, 1,
self.lock_a_p)
}, set())
print('\n'.join([
'{}:\n {}'.format(k, str(c))
for k, c in self.soft_constraints.items()
]))
total_symbols = set()
for c in self.soft_constraints.values():
total_symbols.update(c.expr.free_symbols)
control_symbols = {DiffSymbol(s) for s in total_symbols}
total_symbols.update(control_symbols)
constraints = self.km.get_constraints_by_symbols(total_symbols)
for n, c in constraints.items():
if c.expr in control_symbols:
self.controlled_values[str(c.expr)] = ControlledValue(
c.lower, c.upper, c.expr, 0.001)
else:
self.hard_constraints[n] = c
def diff(self, x):
xdot = DiffSymbol(x)
if xdot in self.diff_symbols:
return self[xdot]
return 0
def __init__(self, name):
super(Lockbox, self).__init__('Lockbox - {}'.format(name))
self.lock_a_p = Position('lock_a')
self.lock_b_p = Position('lock_b')
self.lock_c_p = Position('lock_c')
self.lock_d_p = Position('lock_d')
self.lock_e_p = Position('lock_e')
self.lock_f_p = Position('lock_f')
pos_symbols = {
self.lock_a_p, self.lock_b_p, self.lock_c_p, self.lock_d_p,
self.lock_e_p, self.lock_f_p
}
self.pos_symbols_str = {str(s) for s in pos_symbols}
self.lock_a_v = DiffSymbol(self.lock_a_p)
self.lock_b_v = DiffSymbol(self.lock_b_p)
self.lock_c_v = DiffSymbol(self.lock_c_p)
self.lock_d_v = DiffSymbol(self.lock_d_p)
self.lock_e_v = DiffSymbol(self.lock_e_p)
self.lock_f_v = DiffSymbol(self.lock_f_p)
vel_symbols = {
self.lock_a_v, self.lock_b_v, self.lock_c_v, self.lock_d_v,
self.lock_e_v, self.lock_f_v
}
self.vel_symbols_str = {str(s) for s in vel_symbols}
self.int_rules = {s: s for s in vel_symbols}
b_open_threshold = 0.4
c_open_threshold = 0.6
d_open_threshold = 0.8
e_open_threshold = 1.0
f_open_threshold = 1.2
# Locking rules
# b and c lock a
# d locks b and c
# e locks c and d
# f locks e
a_open_condition = alg_and(
greater_than(self.lock_b_p, b_open_threshold),
greater_than(self.lock_c_p, c_open_threshold))
b_open_condition = greater_than(self.lock_d_p, d_open_threshold)
c_open_condition = alg_and(
greater_than(self.lock_d_p, b_open_threshold + 0.1),
greater_than(self.lock_e_p, e_open_threshold))
d_open_condition = greater_than(self.lock_e_p, e_open_threshold - 0.1)
e_open_condition = greater_than(self.lock_f_p, f_open_threshold)
self.lock_str_labels = [
'a open = $b \\succ {} \\curlywedge c \\succ {}$'.format(
b_open_threshold, c_open_threshold),
'b open = $d \\succ {}$'.format(d_open_threshold),
'c open = $d \\succ {} \\curlywedge e \\succ {}$'.format(
b_open_threshold + 0.1, e_open_threshold),
'd open = $e \\succ {}$'.format(e_open_threshold - 0.1),
'e open = $f \\succ {}$'.format(f_open_threshold)
]
self.recorded_terms = dict(
zip(self.lock_str_labels, [
a_open_condition.expr, b_open_condition.expr,
c_open_condition.expr, d_open_condition.expr,
e_open_condition.expr
]))
# Velocity constraints
self.km.add_constraint(
'lock_a_velocity',
Constraint(-0.4 * a_open_condition, 0.4 * a_open_condition,
self.lock_a_v))
self.km.add_constraint(
'lock_b_velocity',
Constraint(-0.1, 0.1 * b_open_condition, self.lock_b_v))
self.km.add_constraint(
'lock_c_velocity',
Constraint(-0.1, 0.1 * c_open_condition, self.lock_c_v))
self.km.add_constraint(
'lock_d_velocity',
Constraint(-0.1, 0.1 * d_open_condition, self.lock_d_v))
self.km.add_constraint(
'lock_e_velocity',
Constraint(-0.1, 0.1 * e_open_condition, self.lock_e_v))
self.km.add_constraint('lock_f_velocity',
Constraint(-0.4, 0.4, self.lock_f_v))
# Configuration space
self.km.add_constraint(
'lock_b_position',
Constraint(-self.lock_b_p, 0.7 - self.lock_b_p, self.lock_b_p))
self.km.add_constraint(
'lock_c_position',
Constraint(-self.lock_c_p, 0.8 - self.lock_c_p, self.lock_c_p))
self.km.add_constraint(
'lock_d_position',
Constraint(-self.lock_d_p, 0.9 - self.lock_d_p, self.lock_d_p))
self.km.add_constraint(
'lock_e_position',
Constraint(-self.lock_e_p, 1.1 - self.lock_e_p, self.lock_e_p))