Last active
July 27, 2021 08:58
-
-
Save justagist/3530cd05fbc17a6b1ce8832f8f5e2f8c to your computer and use it in GitHub Desktop.
sample simplified hybrid force-motion code
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
# 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 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment