This PR fixes the following issues I have encountered with running inverse kinematics with a scene object as the argument.
When performing a single-tip query, we observe an error message and kinematic constraints are not applied during IK solving.
Link '' not found in model 'my_robot'
[ WARN] [1683770738.195248063]: Position constraint link model not found in kinematic model. Constraint invalid.
The IKQuery object created by the scene object constructor does not perform collision checking with the provided scene, since the constructor does not save the passed in ScenePtr, leading to invalid IK solutions.
Expected Behavior
We should be able to perform IK relative to an object to find a goal pose that can be used to plan a path to grab an object. This IK should obey kinematic constraints and perform collision checking with the scene to ensure the final pose is feasible.
Current Behavior
Creating an IKQuery using the object constructor leads to an error in constraint definition and does not perform collision checking with the scene.
Steps to Reproduce
Create a scene with a robot, goal object, and other obstacles.
Create an IKQuery with the object as a constructor
Run setFromIK using the given IKQuery and examine the console output and visualize the resulting state
Observe that the state is in collision with the environment.
Observe an error message stating that position constraint cannot be defined due to not finding "" link in the kinematic model.
When this request is passed into bool Robot::setFromIK(const IKQuery &query, robot_state::RobotState &state) const, this line attempts to create a new tips object that is set to the Tip frame of the query group.
The updated tips is then used in the call to state.setFromIK to invoke the actual IK here.
However, the original query.tips is also used for other parts of the IK process, including defining constraints for the IK solver here, where query.getAsConstraints invokes void Robot::IKQuery::getMessage() consthere, which uses the query object's tips to set the position and orientation constraints.
In this case, tips[0] is the empty string and results in no kinematic constraints being defined, which causes invalid IK solutions to be produced.
The solution implemented in this PR is to actually copy the query and not just the tips variable as the comment in the code seems to indicate, which will ensure the updated tips variable is used throughout in the calculation.
Alternatively, we can remove the const qualifier of the IKQuery object, but this would be a larger breaking change.
Fixing Collision Checking
When using the IKQuery constructor that takes in an object, it turns out the constructor does not initialize the scene object of the IKQuery, causing the IK solver to use the default collision checking instead of the scene's. I fixed the construtor to properly initialize the Scene pointer as well as expose the verbose parameter for this constructor.
This PR fixes the following issues I have encountered with running inverse kinematics with a scene object as the argument.
IKQueryobject created by the scene object constructor does not perform collision checking with the provided scene, since the constructor does not save the passed inScenePtr, leading to invalid IK solutions.Expected Behavior
We should be able to perform IK relative to an object to find a goal pose that can be used to plan a path to grab an object. This IK should obey kinematic constraints and perform collision checking with the scene to ensure the final pose is feasible.
Current Behavior
Creating an
IKQueryusing the object constructor leads to an error in constraint definition and does not perform collision checking with the scene.Steps to Reproduce
IKQuerywith the object as a constructorsetFromIKusing the given IKQuery and examine the console output and visualize the resulting state""link in the kinematic model.The Fix
Missing constraints
When using the
IKQueryconstructor that takes in an object. A request is constructed with the empty string:""as the tip.When this request is passed into
bool Robot::setFromIK(const IKQuery &query, robot_state::RobotState &state) const, this line attempts to create a new tips object that is set to the Tip frame of the query group.The updated tips is then used in the call to
state.setFromIKto invoke the actual IK here.However, the original
query.tipsis also used for other parts of the IK process, including defining constraints for the IK solver here, wherequery.getAsConstraintsinvokesvoid Robot::IKQuery::getMessage() consthere, which uses thequeryobject'stipsto set the position and orientation constraints.In this case,
tips[0]is the empty string and results in no kinematic constraints being defined, which causes invalid IK solutions to be produced.The solution implemented in this PR is to actually copy the query and not just the tips variable as the comment in the code seems to indicate, which will ensure the updated tips variable is used throughout in the calculation.
Alternatively, we can remove the const qualifier of the
IKQueryobject, but this would be a larger breaking change.Fixing Collision Checking
When using the
IKQueryconstructor that takes in an object, it turns out the constructor does not initialize thesceneobject of theIKQuery, causing the IK solver to use the default collision checking instead of the scene's. I fixed the construtor to properly initialize the Scene pointer as well as expose theverboseparameter for this constructor.