building new primitive for object corner pivoting

Author: taylorott

msg/ControlCommand.msg

@@ -7,11 +7,17 @@ float32 delta_theta
 float32 delta_s_pivot
 float32 delta_s_hand
 
+float32 delta_theta_relative
+float32 delta_theta_object
+
 # absolute commands
 float32 theta
 float32 s_pivot
 float32 s_hand
 
+float32 theta_relative
+float32 theta_object
+
 #jog commands
 float32 delta_x
 float32 delta_y

src/ControlandExecution/interactive_command_module.py

@@ -41,6 +41,7 @@
     'delta_s_hand' : 0.0,
 }
 
+
 delta_rotate_corner_left = {
     'name': 'delta_rotate_left',
     'command_flag' : 1,
@@ -305,6 +306,43 @@
     'delta_tangential': 0.0,
 }
 
+delta_rotate_hand_left_object_corner = {
+    'name': 'delta_rotate_hand_left_object_corner',
+    'command_flag' : 4,
+    'mode' : -1,
+    'delta_theta' : np.pi/50, #np.pi/12, 
+    'delta_s_pivot' : 0.0,
+    'delta_s_hand' : 0.0,
+}
+
+delta_rotate_hand_right_object_corner = {
+    'name': 'delta_rotate_hand_right_object_corner',
+    'command_flag' : 4,
+    'mode' : -1,
+    'delta_theta' : -np.pi/50, #-np.pi/12, 
+    'delta_s_pivot' : 0.0,
+    'delta_s_hand' : 0.0,
+}
+
+delta_rotate_object_left_object_corner = {
+    'name': 'delta_rotate_object_left_object_corner',
+    'command_flag' : 4,
+    'mode' : 0,
+    'delta_theta' : np.pi/50, #np.pi/12, 
+    'delta_s_pivot' : 0.0,
+    'delta_s_hand' : 0.0,
+}
+
+delta_rotate_object_right_object_corner = {
+    'name': 'delta_rotate_object_right_object_corner',
+    'command_flag' : 4,
+    'mode' : 0,
+    'delta_theta' : -np.pi/50, #-np.pi/12, 
+    'delta_s_pivot' : 0.0,
+    'delta_s_hand' : 0.0,
+}
+
+
 
 def on_release(key):
     # try:
@@ -322,18 +360,18 @@ def on_press(key):
 
     global shift_on
     global ctrl_on
-    global alt_on
+    global tab_on
 
     command_msg_dict = None
 
-    not_a_modifier_key = key!=keyboard.Key.shift and key!=keyboard.Key.ctrl and key!=keyboard.Key.alt
+    not_a_modifier_key = key!=keyboard.Key.shift and key!=keyboard.Key.ctrl and key!=keyboard.Key.tab
     # shift_on = keyboard.Key.shift in keys_held_down
     # ctrl_on = keyboard.Key.ctrl in keys_held_down
-    # alt_on = keyboard.Key.alt in keys_held_down
+    # tab_on = keyboard.Key.tab in keys_held_down
 
     if key==keyboard.Key.shift:
         ctrl_on = False
-        alt_on = False
+        tab_on = False
 
         shift_on = not shift_on
 
@@ -344,7 +382,7 @@ def on_press(key):
 
     if key==keyboard.Key.ctrl:
         shift_on = False
-        alt_on = False
+        tab_on = False
 
         ctrl_on = not ctrl_on
 
@@ -353,18 +391,18 @@ def on_press(key):
         else:
             print('ctrl off')
 
-    if key==keyboard.Key.alt:
+    if key==keyboard.Key.tab:
         shift_on = False
         ctrl_on = False
 
-        alt_on = not alt_on
+        tab_on = not tab_on
 
-        if alt_on:
-            print('alt on')
+        if tab_on:
+            print('tab on')
         else:
-            print('alt off')
+            print('tab off')
 
-    modifier_state = shift_on + 2*ctrl_on + 4*alt_on
+    modifier_state = shift_on + 2*ctrl_on + 4*tab_on
 
     # if not_a_modifier_key:
     #     print('modifier_state: ',modifier_state)
@@ -490,6 +528,23 @@ def on_press(key):
             command_msg_dict = jog_move_right
             print('jog_move_right')
 
+    if modifier_state==4:
+        if k == 'left':
+            command_msg_dict = delta_rotate_hand_left_object_corner
+            print('delta_rotate_hand_left_object_corner')
+
+        if k == 'right':
+            command_msg_dict = delta_rotate_hand_right_object_corner
+            print('delta_rotate_hand_right_object_corner')
+
+        if k == 'a':
+            command_msg_dict = delta_rotate_object_left_object_corner
+            print('delta_rotate_object_left_object_corner')
+
+        if k == 'd':
+            command_msg_dict = delta_rotate_object_right_object_corner
+            print('delta_rotate_object_right_object_corner')
+
     if command_msg_dict is not None:
         rm.pub_barrier_func_control_command(command_msg_dict)
 
@@ -533,7 +588,7 @@ def on_press(key):
 
     shift_on = False
     ctrl_on = False
-    alt_on = False
+    tab_on = False
 
     rm = ros_manager()
     rospy.init_node('barrier_func_commands')

src/Helpers/pbal_msg_helper.py

@@ -332,11 +332,17 @@ def command_stamped_to_command_dict(command_msg):
     command_dict['delta_s_pivot']   = command_msg.control_command.delta_s_pivot
     command_dict['delta_s_hand']    = command_msg.control_command.delta_s_hand
 
+    command_dict['delta_theta_relative']     = command_msg.control_command.delta_theta_relative
+    command_dict['delta_theta_object']       = command_msg.control_command.delta_theta_object
+
     # absolute commands
     command_dict['theta']           = command_msg.control_command.theta
     command_dict['s_pivot']         = command_msg.control_command.s_pivot
     command_dict['s_hand']          = command_msg.control_command.s_hand
 
+    command_dict['theta_relative']     = command_msg.control_command.theta_relative
+    command_dict['theta_object']       = command_msg.control_command.theta_object
+
     #additional commands for jogging
     command_dict['delta_x']     = command_msg.control_command.delta_x
     command_dict['delta_y']     = command_msg.control_command.delta_y

src/Modelling/modular_barrier_controller.py

@@ -624,6 +624,7 @@ def torque_right_contact_constraint(self):
         lc = self.l_contact * self.pivot_params['l_contact_multiplier']
         if self.torque_bounds is not None:
             aiq = np.array([self.torque_bounds[0], 0., 1.])
+            print(aiq)
         else:
             aiq = np.array([-lc / 2., 0., 1.])
         biq = -self.pivot_params['torque_margin']
@@ -634,6 +635,7 @@ def torque_left_contact_constraint(self):
         lc = self.l_contact * self.pivot_params['l_contact_multiplier']
         if self.torque_bounds is not None:
             aiq = np.array([-self.torque_bounds[1], 0., -1.])
+            print(aiq)
         else:
             aiq = np.array([-lc / 2., 0., -1.])
         biq = -self.pivot_params['torque_margin']
@@ -668,6 +670,12 @@ def normal_force_min_external_constraint(self):
         biq = 0.
         return aiq, biq, self.pivot_params['tr_min_normal_external'], ['nemn']
 
+    # def radial_force_bounded_external_constraint(self):
+
+    #     radial_vector = robot_contact_wm - external_contact_wm
+        
+
+
     def friction_right_external_constraint(self):
         ''' right (i.e., positive) boundary of friction cone '''
         measured_friction_available = False