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))