Can now get the jacobian of a child link of a move group. #877

BryceStevenWilley · 2018-05-02T02:18:57Z

Description

Originally, if you were to try to get the Jacobian of a link that was at the end of a move group, but not a part of that move group (say a point on a gripper link at the end of a UR robot), then MoveIt would complain that this link was not a part of the move group, even though you should be able to find the jacobian for this point.

This change fixes that, and allows you to get the jacobian of any link that is a child of the move group being queried.

Don't quite have the time to make a test for this, considering there aren't any jacobian tests yet.

Checklist

Required: Code is auto formatted using clang-format
~~Extended the tutorials / documentation, if necessary reference~~
~~Include a screenshot if changing a GUI~~
Optional: Created tests, which fail without this PR reference
Optional: Decide if this should be cherry-picked to other current ROS branches (Indigo, Jade, Kinetic)

rhaschke

I think this PR is way too redundant. It should suffice to skip joints that are not part of the group.
What does group->getVariableGroupIndex(pjm->getName()) return in this case? An exception?

Please better describe the actual error you get when trying to pass a link that's not part of the group.

rhaschke · 2018-05-02T06:44:25Z

moveit_core/robot_state/src/robot_state.cpp

+  while (not group->hasLinkModel(link->getName()))
+  {
+    link = link->getParentLinkModel();
+    if (not link)


It was already checked in line 1126 that link is updated by this group.

Yes, but it doesn't check that this link/its parent joint are a part of the group, which is an assumption made several lines down at group->getVariableGroupIndex(pjm->getName()); (see below comment for full explanation).

I see your point now. I'll make this change soon.

moveit_core/robot_state/src/robot_state.cpp

BryceStevenWilley · 2018-05-02T18:37:10Z

What does group->getVariableGroupIndex(pjm->getName()) return in this case? An exception?

It prints an error, [ERROR] [1525283793.492014912, 313.314000000]: Variable 'robotiq_85_right_finger_tip_joint' is not part of group 'manipulator', and returns -1. The actual error happens several lines down when it tries to index an Eigen matrix with the returned -1. Here's a partial stack trace.

#0  0x00007ffff5356428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
#1  0x00007ffff535802a in __GI_abort () at abort.c:89
#2  0x00007ffff534ebd7 in __assert_fail_base (fmt=<optimized out>, 
    assertion=assertion@entry=0x7ffff44e2598 "startRow >= 0 && BlockRows >= 1 && startRow + BlockRows <= xpr.rows() && startCol >= 0 && BlockCols >= 1 && startCol + BlockCols <= xpr.cols()", 
    file=file@entry=0x7ffff44e2568 "/usr/include/eigen3/Eigen/src/Core/Block.h", line=line@entry=134, 
    function=function@entry=0x7ffff44ee120 <Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 3, 1, false>::Block(Eigen::Matrix<double, -1, -1, 0, -1, -1>&, long, long)::__PRETTY_FUNCTION__> "Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>::Block(XprType&, Eigen::Index, Eigen::Index) [with XprType = Eigen::Matrix<double, -1, -1>; int BlockRows = 3; int BlockCols = 1; bool InnerPane"...) at assert.c:92
#3  0x00007ffff534ec82 in __GI___assert_fail (
    assertion=0x7ffff44e2598 "startRow >= 0 && BlockRows >= 1 && startRow + BlockRows <= xpr.rows() && startCol >= 0 && BlockCols >= 1 && startCol + BlockCols <= xpr.cols()", 
    file=0x7ffff44e2568 "/usr/include/eigen3/Eigen/src/Core/Block.h", line=134, 
    function=0x7ffff44ee120 <Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 3, 1, false>::Block(Eigen::Matrix<double, -1, -1, 0, -1, -1>&, long, long)::__PRETTY_FUNCTION__> "Eigen::Block<XprType, BlockRows, BlockCols, InnerPanel>::Block(XprType&, Eigen::Index, Eigen::Index) [with XprType = Eigen::Matrix<double, -1, -1>; int BlockRows = 3; int BlockCols = 1; bool InnerPane"...) at assert.c:101
#4  0x00007ffff444e7c8 in Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 3, 1, false>::Block (
    this=0x7fffb1412e90, xpr=..., startRow=0, startCol=4294967295)
    at /usr/include/eigen3/Eigen/src/Core/Block.h:133
#5  0x00007ffff4448249 in Eigen::DenseBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >::block<3, 1> (
    this=0x7fffb1413260, startRow=0, startCol=4294967295)
    at /usr/include/eigen3/Eigen/src/plugins/BlockMethods.h:725
#6  0x00007ffff443b282 in moveit::core::RobotState::getJacobian (this=0x7fffa000dda0, group=0xb5aa40, 
    link=0xaca5d0, reference_point_position=..., jacobian=..., use_quaternion_representation=false)
    at /home/brycew/Desktop/moveit_ws/src/moveit/moveit_core/robot_state/src/robot_state.cpp:1175
#7  0x00007fffd8464fbd in moveit::core::RobotState::getJacobian (this=0x7fffa000dda0, group=0xb5aa40, 
    link=0xaca5d0, reference_point_position=..., jacobian=..., use_quaternion_representation=false)
    at /home/brycew/Desktop/moveit_ws/src/moveit/moveit_core/robot_state/include/moveit/robot_state/robot_state.h:

I think this PR is way too redundant. It should suffice to skip joints that are not part of the group.

I see your point now, I tend to prefer double checking things when unsure. I made the suggested changes, and they work, but I won't be able to push them to this PR until later tonight.

rhaschke

Thanks for simplifying this.

v4hn · 2018-05-15T16:28:38Z

The change looks good.

This still misses the case where a static sibling link of a link in the group is requested.
The required information is already collected in LinkModel::getAssociatedFixedTransforms.
Would it be too much to ask for this extension here too? :-)
If it is, I'm happy to merge this request now and file an issue for the rest.

BryceStevenWilley · 2018-05-20T16:27:18Z

@v4hn I'd be happy to add that case, but let me make sure I understand what case you are talking about. Say for 3 links, A, B, and C, where A is a parent to both B and C with fixed joints between A-B and A-C, and B is the start of some joint group. If the Jacobian of a point in C is requested, we should recognize that C is a static sibling of B and compute the Jacobian from there?

Given my understanding is correct, I'm not sure what to do, as getAssociatedFixedTransforms only contains information between a link and its children (see computeFixedTransforms, which only iterates over children), not information between a link and any of its siblings.

If you are talking about the case where a link is a fixed child of the joint group, the current change should catch that, without needing getAssociatedFixedTransforms.

v4hn · 2018-05-23T12:03:10Z

Say for 3 links, A, B, and C, where A is a parent to both B and C with fixed joints between A-B and A-C, and B is the start of some joint group. If the Jacobian of a point in C is requested, we should recognize that C is a static sibling of B and compute the Jacobian from there?

Yes.

Given my understanding is correct, I'm not sure what to do, as getAssociatedFixedTransforms only contains information between a link and its children

Thank you for pointing this out! I rewrote this code two months ago, but apparently I forgot to submit a pull-request for it. I just created #912.

Once this is merged it would be great to add the sibling capability to the code here!
I don't see a reason though to hold this request until then, so I will merge this now.

Thank you for the contribution already!

Can now get the jacobian of a child link of a move group.

v4hn · 2018-05-23T12:04:47Z

cherry-picked to melodic

Can now get the jacobian of a child link of a move group.

rhaschke · 2018-10-21T20:49:58Z

@BryceStevenWilley @v4hn Is there anything else to do here, since #912 is merged now?
See #877 (comment)

BryceStevenWilley · 2018-10-22T00:56:43Z

Yes, now that #912 is in, we should use LinkModel::getAssociatedTransforms() in addition to JointModel::getParentLinkModel() to iterate over links that are potentially in the jacobian group. I can get to this at some point this week.

rhaschke · 2018-10-22T07:06:57Z

Great, thanks!

BryceStevenWilley · 2018-11-04T03:45:41Z

So I was working on finishing up using getAssociatedTransforms for static siblings, and realized a few things:

a static sibling link that is updated by a group must be a descendant of an earlier link in the group
A link can be a static sibling of a group and not a descendant only in the case described in the discussion above (a to b and to c, both fixed joints, b's parent joint is the first in a group), and in this cane, the Jacobian of that link is trivially the 0 matrix, as moving any joint in the group will not move c. In this case, JointModelGroup::isLinkUpdated() returns false for c, which is correct: c will not move.

@v4hn, am I misunderstanding something, or is the only case getJacobian missing right now the latter case? If that is the only case, is there any reason someone would want a trivial 0 jacobian for a sibling link like that?

Can now get the jacobian of a child link of a move group.

2a3b09c

rhaschke requested changes May 2, 2018

View reviewed changes

BryceStevenWilley added 2 commits May 2, 2018 19:04

Removed redundancy in jacobian group fix.

bae0116

fixup! Removed redundancy in jacobian group fix.

d56d217

rhaschke approved these changes May 3, 2018

View reviewed changes

rhaschke added the awaits 2nd review one maintainer approved this request label May 3, 2018

v4hn mentioned this pull request May 23, 2018

include fixed tfs to siblings in links #912

Merged

v4hn merged commit ad9e54a into moveit:kinetic-devel May 23, 2018

v4hn pushed a commit that referenced this pull request May 23, 2018

Can now get the jacobian of a child link of a move group. (#877)

66822bc

Can now get the jacobian of a child link of a move group.

BryceStevenWilley deleted the jacobian-not-in-group branch May 24, 2018 04:03

dg-shadow pushed a commit to shadow-robot/moveit that referenced this pull request Jul 30, 2018

Can now get the jacobian of a child link of a move group. (moveit#877)

3d9304b

Can now get the jacobian of a child link of a move group.

rhaschke removed the awaits 2nd review one maintainer approved this request label Oct 21, 2018

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Can now get the jacobian of a child link of a move group. #877

Can now get the jacobian of a child link of a move group. #877

BryceStevenWilley commented May 2, 2018

rhaschke left a comment

rhaschke May 2, 2018

BryceStevenWilley May 2, 2018 •

edited

Loading

BryceStevenWilley commented May 2, 2018

rhaschke left a comment

v4hn commented May 15, 2018 •

edited

Loading

BryceStevenWilley commented May 20, 2018

v4hn commented May 23, 2018

v4hn commented May 23, 2018

rhaschke commented Oct 21, 2018

BryceStevenWilley commented Oct 22, 2018

rhaschke commented Oct 22, 2018

BryceStevenWilley commented Nov 4, 2018

Can now get the jacobian of a child link of a move group. #877

Can now get the jacobian of a child link of a move group. #877

Conversation

BryceStevenWilley commented May 2, 2018

Description

Checklist

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke May 2, 2018

Choose a reason for hiding this comment

BryceStevenWilley May 2, 2018 • edited Loading

Choose a reason for hiding this comment

BryceStevenWilley commented May 2, 2018

rhaschke left a comment

Choose a reason for hiding this comment

v4hn commented May 15, 2018 • edited Loading

BryceStevenWilley commented May 20, 2018

v4hn commented May 23, 2018

v4hn commented May 23, 2018

rhaschke commented Oct 21, 2018

BryceStevenWilley commented Oct 22, 2018

rhaschke commented Oct 22, 2018

BryceStevenWilley commented Nov 4, 2018

BryceStevenWilley May 2, 2018 •

edited

Loading

v4hn commented May 15, 2018 •

edited

Loading