Trajectory tracking under force mode

Technical Questions
1

Hi,

I have question regarding trajectory tracking with servoJ and force mode. I need to follow pre-recorded trajectory with force maximum of 30N.

My code looks like this

 Program
   BeforeStart
     zero_ftsensor()
     MoveJ
       Waypoint_1
     Wait: 0.01
     joints≔get_actual_joint_positions()
     write_output_boolean_register(0, False )
     popup("RUN THE PYTHON CODE",title="RUN",blocking= True )
     state≔0
   Robot Program
     write_output_boolean_register(0, True )
     sync()
     Loop state<3
       Loop state≟1
         MoveJ
           Wait: 0.01
           joints
         write_output_boolean_register(0,False)
         sync()
       Loop state≟2
         servoj(joints, 0, 0, 0.008, 0.2, 300)
       Loop state≟3
         end_force_mode()
         Halt
     sync()
   Thread_1
     tmp≔p[0,0,0,0,0,0]
     Loop
       Script: pose_register.script
       joints≔get_inverse_kin(p[tmp[0],tmp[1],tmp[2],tmp[3],tmp[4],tmp[5]],get_actual_joint_positions())
       sync()
       state≔read_input_integer_register(0)
       sync()
   Thread_2
     force_mode_set_damping(0.1)
     force_mode(tool_pose(),[0,0,1,0,0,0],[0.0,0.0,30,0.0,0.0,0.0],2,[0.15,0.15,0.15,1.57,1.57,1.57])
     sync()

Problem is that robot is not following path as is should in Z axis.
Screenshot from 2022-11-01 15-20-15
Screenshot from 2022-11-01 15-20-34

Screenshot from 2022-11-01 15-20-22

As you can see from my plots robot is not following trajectory in Z axis (Y and X are correct). Approximately at time-stamp of 900 you can see contact with surface. My question is how to make the robot follow the path with maximum of speed of 0.15m/s and maximum of force of 30N (± 10N). Damping and gain scaling parameters are not doing the trick.

I saw already some implementations of updating wrench parameter in force_mode() based on PD controller but I need to apply stable 30N.

Any suggestions ?
Thanks :slight_smile: