diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 5cdf35e11..496dfe62f 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -311,11 +311,7 @@ end # Get the measured wrench at the tool flange def get_flange_wrench(): local ft = get_tcp_force() #Tcp force returns the force and torques at the tool flange with base orientation - local t_end_base = pose_trans(get_target_tcp_pose(), pose_inv(get_tcp_offset())) - local current_rot_in_tool = pose_inv(p[0, 0, 0, t_end_base[3], t_end_base[4], t_end_base[5]]) - local f = pose_trans(current_rot_in_tool, p[ft[0], ft[1], ft[2], 0, 0, 0]) - local t = pose_trans(current_rot_in_tool, p[ft[3], ft[4], ft[5], 0, 0, 0]) - return [f[0], f[1], f[2], t[0], t[1], t[2]] + return [ft[0], ft[1], ft[2], ft[3], ft[4], ft[5]] end thread resolvedAccelerationThread(): @@ -325,7 +321,7 @@ thread resolvedAccelerationThread(): while control_mode == MODE_RESOLVED_ACCELERATION: # We do the transformation in the tool flange frame. local external_torque = transpose(get_jacobian(tcp=p[0,0,0,0,0,0])) * get_flange_wrench() - local tau = get_mass_matrix(include_rotor_inertia=True) * cmd_acceleration + get_coriolis_and_centrifugal_torques() + external_torque + local tau = get_mass_matrix(include_rotor_inertia=True) * cmd_acceleration + get_coriolis_and_centrifugal_torques() - external_torque directTorqueControlCall(tau) end textmsg("ExternalControl: Resolved acceleration thread ended") @@ -338,7 +334,7 @@ thread resolvedAccelerationThread(): while control_mode == MODE_RESOLVED_ACCELERATION: # We do the transformation in the tool flange frame. local external_torque = transpose(get_jacobian(tcp=p[0,0,0,0,0,0])) * get_flange_wrench() - local tau = get_mass_matrix(include_rotor_inertia=True) * cmd_acceleration + get_coriolis_and_centrifugal_torques() + external_torque + local tau = get_mass_matrix(include_rotor_inertia=True) * cmd_acceleration + get_coriolis_and_centrifugal_torques() - external_torque directTorqueControlCall(tau) end textmsg("ExternalControl: Resolved acceleration thread ended")