Resolved acceleration controller, second itr#469
Resolved acceleration controller, second itr#469urrsk wants to merge 3 commits intoUniversalRobots:masterfrom
Conversation
Codecov Report✅ All modified and coverable lines are covered by tests. Additional details and impacted files@@ Coverage Diff @@
## master #469 +/- ##
==========================================
+ Coverage 76.89% 76.97% +0.08%
==========================================
Files 110 110
Lines 5821 5820 -1
Branches 2576 2576
==========================================
+ Hits 4476 4480 +4
+ Misses 1006 998 -8
- Partials 339 342 +3
Flags with carried forward coverage won't be shown. Click here to find out more. ☔ View full report in Codecov by Sentry. |
|
@FilipposSot I have restructure your code. Please review and test this. |
urrsk
force-pushed
the
resolved_acceleration_controller_rsk
branch
3 times, most recently
from
04301a8 to
be9c3c1
Compare
Extract the version-dependent direct_torque call logic from torqueThread() into a standalone directTorqueControlCall(tau) function. This allows reuse of the same logic in other control threads without code duplication.
urrsk
force-pushed
the
resolved_acceleration_controller_rsk
branch
from
be9c3c1 to
9f26a84
Compare
FilipposSot
left a comment
FilipposSot
left a comment
There was a problem hiding this comment.
Thanks for incorporating this. I left my thoughts on the transformations which you added since my understanding is that these should not be necessary.
| textmsg("ExternalControl: Starting resolved acceleration thread") | ||
| 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() |
There was a problem hiding this comment.
I have taken a look at this and want to clarify a bit on the coordinate conventions since I am not sure this additional rotation is necessary from my understanding.
In my initial PR (#450) I am calculating external_torque = J_flange_in_base_frame^T * F_flange_in_base_frame
I have looked at the documentation and for each of J_flange_in_base_frame and F_flange_in_base_frame here is what I can conclude for each of the terms. There may be some misunderstanding on my end potentially.
J_flange_in_base_frame: get_jacobian is used but tcp should be specified as the identity (all zeros) so no offset from the robot flange. I had originally missed this detail. This is by default calculated with respect to the robot base frame (ie velocities measured in the robot base frame).
F_flange_in_base_frame: Here it seems to me that no additional rotation of the wrench is necessary from the output from get_tcp_force(). In particular reading the documentation I see “...the forces: Fx, Fy, and Fz in Newtons and the torques: TRx, TRy and TRz … are all measured at the tool flange with the orientation of the robot base coordinate system.” In other words it seems to me we don’t need to rotate the wrench.
This was both the J and F terms are both defined in the same frame.
In fact based on this assumption my original PR worked properly during my testing. In particular the controller was exerting forces opposing external disturbances (poking the tool, moving into contact).
| 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 |
There was a problem hiding this comment.
The sign of the external_torque term here should be negative. We need to cancel the external disturbances.
2 participants
Restructured version of #450