Fixing joints moving to default position when their target is not set #773
Conversation
…_to_joint_value_target
jeferal
changed the title
| @@ -101,16 +105,63 @@ def __init__(self, name): | |||
|
|
|||
| controller_list_param = rospy.get_param("/move_group/controller_list") | |||
|
|
|||
| robot_description = rospy.get_param("/robot_description") | |||
| self._joint_limits = dict() | |||
There was a problem hiding this comment.
better to have this self._joint_limits = {} to be compliant with the new linter (you have also a dictionary init below done with {} style :)
| self.tf_buffer = tf2_ros.Buffer() | ||
| self.listener = tf2_ros.TransformListener(self.tf_buffer) | ||
|
|
||
| threading.Thread(None, rospy.spin) | ||
|
|
||
| self._wait_for_set_points() | ||
|
|
||
| def _read_joint_limits(self, robot_description): |
There was a problem hiding this comment.
I think this is supposed to happen only once right? Maybe would be better to rename it to something like _initialize_joint_limits and have then a getter like get_joint_limits (returning self._joint_limits)
There was a problem hiding this comment.
which seems you have done below :) skip the getter part of the comment :)
| if child.nodeType is child.TEXT_NODE: | ||
| continue | ||
| if child.localName == 'joint': | ||
| jtype = child.getAttribute('type') |
There was a problem hiding this comment.
maybe joint_type instead jtype? same in 144
| if jtype == 'continuous': | ||
| minval = -pi | ||
| maxval = pi | ||
| else: |
There was a problem hiding this comment.
what is the joint_type passed in this else?
There was a problem hiding this comment.
There was a problem hiding this comment.
ok, thanks
can you then just move the name = child.getAttribute('name') before the first 'if' to slightly increase readability?
There was a problem hiding this comment.
the attribute name seems to be particular for nodes of type joint
sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
Outdated
Show resolved
Hide resolved
sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
Outdated
Show resolved
Hide resolved
sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
Outdated
Show resolved
Hide resolved
| joint_member = joint_name[3:-2] | ||
| joint_number = joint_name[5:] |
There was a problem hiding this comment.
Add as comment the structure of joint_name, or use any() in the if below if you can :)
There was a problem hiding this comment.
Maybe a regex with a substring match?
| @@ -15,27 +15,31 @@ | |||
| # with this program. If not, see <http://www.gnu.org/licenses/>. | |||
|
|
|||
| from __future__ import absolute_import | |||
There was a problem hiding this comment.
Don't need this anymore
There was a problem hiding this comment.
If I'm not wrong, this repo still has the master branch of the linter , so if I remove this line, the tests might fail
sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
Outdated
Show resolved
Hide resolved
sr_robot_commander/src/sr_robot_commander/sr_robot_commander.py
Outdated
Show resolved
Hide resolved
| joint_member = joint_name[3:-2] | ||
| joint_number = joint_name[5:] |
There was a problem hiding this comment.
Maybe a regex with a substring match?
| """ | ||
| joint_names_group = self._move_group_commander.get_active_joints() | ||
| with self._set_points_lock: | ||
| for i in range(len(msg.joint_names)): |
There was a problem hiding this comment.
Consider using enumerate()
There was a problem hiding this comment.
Thank you, I changed it
| self._move_group_commander.set_joint_value_target(joint_states_cpy) | ||
| self._move_group_commander.go(wait=wait) |
There was a problem hiding this comment.
It looks like go function uses the current state internally, I tried to find the line of code that does that, but I could not. By performing tests, I could not reproduce the problem on planning when an object is grasped and that makes me think that is using the current state instead of the state set when using .set_start_satate
| rospy.wait_for_service('compute_ik') | ||
| self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) | ||
| self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK) | ||
|
|
||
| controller_list_param = rospy.get_param("/move_group/controller_list") | ||
| self._allowed_start_tolerance = rospy.get_param("/move_group/trajectory_execution/allowed_start_tolerance", 0.1) |
There was a problem hiding this comment.
This module is common for arm and hand (it's the parent of HandCommander and ArmCommander).
How do we distinguish between allowed_start_tolerance for arm (likely we want it quite low) and hand?
There was a problem hiding this comment.
In there I am only reading that parameter. When launching roslaunch sr_robot_launch sr_ur_arm_hand.launch it seems (at least in simulation) that allowed_start_tolerance is set in here. And it looks like there is only that parameter for any group:
Correct me if I am wrong please.
| """ | ||
| # Get joint names of the group | ||
| joint_names_group = self._move_group_commander.get_active_joints() | ||
| trajectory_controllers_subscribed = [] |
There was a problem hiding this comment.
Maybe call it trajectory_controllers_state_subscribed
| # Get joint names of the group | ||
| joint_names_group = self._move_group_commander.get_active_joints() | ||
| trajectory_controllers_subscribed = [] | ||
| j0_position_controllers_subscribed = [] |
There was a problem hiding this comment.
Maybe j0_position_controllers_state_subscribed
| set_point_j2 = raw_set_points[joint_2_name] | ||
| else: | ||
| # Split j0 given state of j1 and j2 | ||
| set_point_j1 = state_j1 * set_point_j0 / (state_j1 + state_j2) |
There was a problem hiding this comment.
Not sure that this is necessary.
If we plan to get a separate set point for J1 and J2, why don't we take the one form the trajectory controller state?
That is the actual one that get set when one uses trajectory messages.
There was a problem hiding this comment.
We should proably rethink if we need to listen to the J0 position controllers at all.
| else: | ||
| if "J0" not in joint_name: | ||
| # Avoind adding set points of J0 to the output | ||
| set_points.update({joint_name: raw_set_points[joint_name]}) |
There was a problem hiding this comment.
Maybe this method only needs to do this bit?
|
|
||
| def fix_set_points_if_far_from_state(self, set_points, robot_state): | ||
| """ | ||
| Updates the set points with the current stateof the robot if the set point |
|
|
||
| # Update set points | ||
| for joint_name in list(set_points.keys()): | ||
| if abs(set_points[joint_name] - current_state[joint_name]) > self._allowed_start_tolerance: |
There was a problem hiding this comment.
We might want the user to be able to specify the tolerance for every use case.
e.g. if you are grasping a big object you might want to allow J3 and J0 set points to be very far from the current positions (set point inside the object while the current pos is on the surface).
Otherwise this "fixing" could result in a dropped object.
| self._are_set_points_ready = True | ||
| self._set_points_cv.notifyAll() | ||
|
|
||
| def _set_point_j0_cb(self, msg, joint_name): |
There was a problem hiding this comment.
As mentioned before, not really sure if we need to know the J0 goals at all.
Given that we know the J1 and J2 goals (because this whole RobotCommander relies on having a JointTrajectoryposition controller running), maybe we don't need this?
There was a problem hiding this comment.
I think we took the decision of listening to J0's set points because the J1 and J2 set points because of the underactuation might be far from the real state and also outside of the joint limits. For instance, when the set point of FFJ2 is 90deg and the set point of FFJ1 is ~60deg I find this:
If I directly set these set points as the target state when planning, moveit will throw the error: target state is outside of the bounds. I think this also depends on a tolerance that is set through this method.
However, since we also encountered the "problem" of the set points being far from the state when grasping an object and added the "fix" of checking if the set point is far from the state and if it is, use the state then listening to j0's might not be useful.
I also agree that perhaps we should rethink of this whole architecture again.
| self._joints_effort = {n: v for n, v in | ||
| zip(joint_state.name, joint_state.effort)} |
There was a problem hiding this comment.
side note -> this can be just self._joints_effort = {zip(joint_state.name, joint_state.effort)}
Proposed changes
DO NOT MERGE YET.
User story: https://shadowrobot.atlassian.net/browse/SP-376
PR to fix the issue desribed in the user story. The issue is that when planning in joint space through the robot commander, for the joints whose target is not specified, a plan would be created from the current state to the default target value, which is 0 or the value in the middle of the range of the joint if 0 is not contained.
This PR uses the set points of the controllers to set the start state of the plan as well as the target state. It needs more testing, unit tests for the hand. In the user story you can see the tests than have been done.
The set points are read through subscriptions to the trajectory controllers for all the joints except from J2 and J1 of the fingers. For these joints, the set point is read from the position controller of J0 and this value is splitted by calculating the underactuation ration from J1 and J2 position states.
If the hand is grasping an object, the joint states might be far from the set points since the object could be not allowing the joint to reach the goal position. In this case, if the difference between the start state of the trajectory (set points) and the state is higher than the allowed_start_tolerance parameter then moveit would throw an error. This is why a function has been added to check if the set point is far from the state and update it with the state if so.
Checklist
Before posting a PR ensure that from each of the below categories AT LEAST ONE BOX HAS BEEN CHECKED. If more than one category is applicable then more can be checked. Also ensure that the proposed changes have been filled out with relevant information for reviewers.
Tests
Documentation