Dev taskspace control #200
Conversation
latest changes to make move_2_cart a service
…nts / rotation only needs to be verified
…OS connection (publisher)
|
I should mention that, with this feature, 2 service messages should be added to the lbr_fri_idl package, so that the move_2_cart can work properly. I can create a PR against that as well if you want to accept this PR. |
| @@ -0,0 +1,527 @@ | |||
| '''Collection of functions/types from the ASBR class taken in Spring 2022 | |||
There was a problem hiding this comment.
hi @OmidRezayof and thank you for the PR. My issue here is still that these utilities should not live inside this repository but rather by a Python library that is provided externally.
There was a problem hiding this comment.
Thank you for your reply @mhubii .
I understand your point. I'm sure there are alternatives for this in external libraries but I couldn't find a suitable one. I can contact the original author of this library and see if he decides to make it as an installable package.
I can change your other comments but for this one I don't think I would have a quick solution.
Let me know if there was anything I could do! Just wanted to contribute to this repo and help other people who were looking for this functionality! : )
| @@ -41,6 +41,8 @@ def __call__(self, lbr_state: LBRState, dt: float) -> LBRJointPositionCommand: | |||
| self._jacobian = self._jacobian_func(self._q) | |||
| self._jacobian_inv = np.linalg.pinv(self._jacobian, rcond=0.1) | |||
| self._f_ext = self._jacobian_inv.T @ self._tau_ext | |||
| # self._f_ext[2]+= 38.0 #FOR SHEELA'S DRILL | |||
| @@ -49,13 +49,18 @@ def _on_lbr_state(self, lbr_state: LBRState) -> None: | |||
| self._lbr_joint_position_command.joint_position[ | |||
| 3 | |||
| ] += self._amplitude * math.sin(self._phase) | |||
|
|
|||
| self._lbr_joint_position_command.joint_position[ | |||
| @@ -1,8 +1,8 @@ | |||
| # these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface | |||
| hardware: | |||
| fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro | |||
| major_version: 1 | |||
| minor_version: 15 | |||
| major_version: 2 | |||
There was a problem hiding this comment.
this ideally needs to be reversed
Hi @mhubii
Hope you're doing well.
I cleaned up the task space control and created a new branch and a new PR.
This branch is currently up-to-date with all the commits on the lbr-stack/lbr_fri_ros2_stack/humble and should be able to merge.
The main features of this would be:
The motion planning is done in the new code move_2_cart.py. This code receives the desired pose (s) in geometry_msgs.Pose format and will perform the motion planning. The rotational motion planning was done using SLERP (Spherical Linear Interpolation) which performs the motion planning in quaternion space, eliminating Euler Angle singularities.
I have also added a move_to_pose_client.py as a demo for this.
I hope this comes useful to the community.