1.Exit if control force fails;2.Cancel counter string

Author: SchoolMater

example/wrapper/common/8003-force_control.py

@@ -46,33 +46,35 @@
 
 # set pid parameters for force control
 Kp = 0.005  # range: 0 ~ 0.05
-Ki = 0.00006 # range: 0 ~ 0.0005
-Kd = 0.000  # range: 0 ~ 0.05
-v_max = 100.0 # max adjust velocity(mm/s), range: 0 ~ 200
-arm.set_force_control_pid([Kp]*6, [Ki]*6, [Kd]*6, [v_max]*6)
+Ki = 0.00005 # range: 0 ~ 0.0005
+Kd = 0.05  # range: 0 ~ 0.1
+linear_v_max = 200.0 # max adjust velocity(mm/s), range: 0 ~ 200
+rot_v_max = 0.35 # rad/s range: 0~pi/4
+arm.set_force_control_pid([Kp]*6, [Ki]*6, [Kd]*6, [linear_v_max, linear_v_max, linear_v_max, rot_v_max, rot_v_max, rot_v_max])
 
 ref_frame = 1        # 0 : base , 1 : tool
 force_axis = [0, 0, 1, 0, 0, 0] # only control force along z axis
 # MAKE SURE reference frame and force target sign are correct !!
 force_ref = [0, 0, 5.0, 0, 0, 0] # the force(N) that xArm will apply to the environment
-arm.config_force_control(ref_frame,  force_axis, force_ref, [0]*6) # limits are reserved, just give zeros
+code = arm.config_force_control(ref_frame,  force_axis, force_ref, [0]*6) # limits are reserved, just give zeros
+if code == 0 and arm.error_code == 0:
 
-# enable ft sensor communication
-arm.ft_sensor_enable(1)
+    # enable ft sensor communication
+    arm.ft_sensor_enable(1)
 
-# will overwrite previous sensor zero and payload configuration
-arm.ft_sensor_set_zero() # remove this if zero_offset and payload already identified & compensated!
-time.sleep(0.2) # wait for writing zero operation to take effect, do not remove
+    # will overwrite previous sensor zero and payload configuration
+    arm.ft_sensor_set_zero() # remove this if zero_offset and payload already identified & compensated!
+    time.sleep(0.2) # wait for writing zero operation to take effect, do not remove
 
-# move robot in force control
-arm.ft_sensor_app_set(2)
-# will start after set_state(0)
-arm.set_state(0)
+    # move robot in force control
+    arm.ft_sensor_app_set(2)
+    # will start after set_state(0)
+    arm.set_state(0)
 
-# keep for 5 secs
-time.sleep(5)
+    # keep for 5 secs
+    time.sleep(5)
 
-# remember to reset ft_sensor_app when finished
-arm.ft_sensor_app_set(0)
-arm.ft_sensor_enable(0)
+    # remember to reset ft_sensor_app when finished
+    arm.ft_sensor_app_set(0)
+    arm.ft_sensor_enable(0)
 arm.disconnect()

xarm/tools/blockly/_blockly_base.py

@@ -214,7 +214,7 @@ def _get_condition_expression(self, value_block, arg_map=None):
             direction = fields[0].text
             return 'self._arm.get_joints_torque()[1][{}]'.format(direction_li.index(direction))
         elif block.attrib['type'] == 'get_counter':
-            return 'str(self._arm.count)'
+            return 'self._arm.count'
 
 
     def __get_logic_compare(self, block, arg_map=None):