def __init__(self, path_obj, path_other=0):
contact_name = ('contact', ) + path_obj + (
obj_to_obj_infix, ) + path_other
self.on_a_x = Position((contact_name + ('onA', 'x')).to_symbol())
self.on_a_y = Position((contact_name + ('onA', 'y')).to_symbol())
self.on_a_z = Position((contact_name + ('onA', 'z')).to_symbol())
self.on_b_x = Position((contact_name + ('onB', 'x')).to_symbol())
self.on_b_y = Position((contact_name + ('onB', 'y')).to_symbol())
self.on_b_z = Position((contact_name + ('onB', 'z')).to_symbol())
self.normal_x = Position((contact_name + ('normal', 'x')).to_symbol())
self.normal_y = Position((contact_name + ('normal', 'y')).to_symbol())
self.normal_z = Position((contact_name + ('normal', 'z')).to_symbol())
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))
#!/usr/bin/env python
import rospy
from kineverse.gradients.diff_logic import Position
from kineverse_msgs.msg import ValueMap as ValueMapMsg
if __name__ == '__main__':
rospy.init_node('fetch_pose_publisher')
pub = rospy.Publisher('fetch/state', ValueMapMsg, queue_size=1)
poses = [
{
Position("fetch__torso_lift_joint"): 0.1,
Position("fetch__shoulder_pan_joint"): 1.32,
Position("fetch__shoulder_lift_joint"): 1.40,
Position("fetch__upperarm_roll_joint"): -0.2,
Position("fetch__elbow_flex_joint"): 1.72,
Position("fetch__forearm_roll_joint"): 0.0,
Position("fetch__wrist_flex_joint"): 1.66,
Position("fetch__wrist_roll_joint"): 0.0,
Position("fetch__head_tilt_joint"): 0.4,
Position("fetch__head_pan_joint"): -0.6,
Position("fetch__r_gripper_finger_joint"): 0.05
},
{
Position("fetch__torso_lift_joint"): 0.0,
Position("fetch__shoulder_pan_joint"): 0.0,
Position("fetch__shoulder_lift_joint"): 0.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))
