sample simplified hybrid force-motion code

hybrid_force_motion_control.py

  # feedback control loop for hybrid force motion control (simplified)
  def compute_cmd(self, time_elapsed=None):

        robot_state    = robot.state() # from FRI

        # calculate the jacobian of the end effector
        jac_ee         = robot_state['jacobian']

        # get position of the end-effector
        curr_pos = robot_state['ee_point']
        curr_ori = robot_state['ee_ori']

        curr_vel       = robot_state['ee_vel']
        curr_omg       = robot_state['ee_omg']

        curr_force = robot_state['tip_state']['force']
        curr_torque = robot_state['tip_state']['torque']

        delta_pos      = self._goal_pos - curr_pos
        delta_vel      = self._goal_vel - curr_vel
        delta_force    = self._force_dir.dot(self._goal_force - curr_force) # _torque_dir is a vector/matrix representing direction of force control (eg. [0,0,1] for control along Z); should be complementary to _pos_p_dir
        delta_torque   = self._torque_dir.dot(self._goal_torque - curr_torque) # _torque_dir is a vector/matrix representing direction of torque control

        if self._goal_ori is not None:
            delta_ori  = quatdiff3(curr_ori, self._goal_ori)
        else:
            delta_ori  = np.zeros(delta_pos.shape)
        
        delta_ori      = self._pos_o_dir.dot(delta_ori)

        delta_omg      = self._pos_o_dir.dot(self._goal_omg - curr_omg)

        if np.linalg.norm(curr_force) < 3.:
            force_control = self._force_dir.dot(np.sign(self._goal_force)*5.)

        else:
            force_control = self._force_dir.dot(self._kp_f.dot(delta_force) + curr_force) # this is a simple P control (typically you might want to implement PI control for force) 

        position_control  = self._pos_p_dir.dot(self._kp_p.dot(delta_pos) + self._kd_p.dot(delta_vel)) # PD control for motion along positional direction; you can implement variable impedance here if needed
        
        x_des = position_control + force_control # total translational control ## force control + positional motion control

        # COmpute control for orientation components (orientational position control and end-effector torque control if needed)
        if self._orientation_ctrl:
          
            ori_pos_ctrl = self._pos_o_dir.dot(self._kp_o.dot(delta_ori) + self._kd_o.dot(delta_omg)) # PD control for motion along orientation direction; you can implement variable impedance here if needed

            torque_f_ctrl = self._torque_dir.dot(self._kp_t.dot(delta_torque) + self._goal_torque) # this is a simple P control (typically you might want to implement PI control for torque) 

            omg_des =  ori_pos_ctrl + torque_f_ctrl  # total orientational control command

        else:

            omg_des = np.zeros(3)

        f_ee = np.hstack([x_des, omg_des]) # Desired end-effector wrench

        u = np.dot(jac_ee.T, f_ee) # tau = J^T . F

        if self._use_null_ctrl:
			# the below is only one way of implementing null-space control. This tries to keep the robot close to neutral pose as a secondary goal
            null_space_filter = self._null_Kp.dot(np.eye(7) - jac_ee.T.dot(np.linalg.pinv(jac_ee.T, rcond=1e-3)))

            self._cmd = self._cmd + null_space_filter.dot(self._robot._tuck-robot_state['position'])
                
        return self._cmd