Multiple source frames for a single program?

[naren-sriram-robo]

Hi, I have a welding like scenario in which I need to pass different source frames for each waypoint and the tool frame (which is the target frame) must align with this source frame. I have my own urdfs, source and target frames.

I understand this is the part where we pass the source and target frames:

CompositeInstruction program( "cartesian_program", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "grinder_frame", "tool2"));

I wanted to know if it is possible to pass different source frames as a part of this program or create multiple programs (1 for each waypoint) and pass it to the same request. Any help would be greatly appreciated and I can answer any further questions.

Thank you.

[Levi-Armstrong]

Sure, the one passed to the CompositeInstruction is a global one which is used if a move instruction that does not contain its own ManipulatorInfo. In your case you would assign a manipulator info to each move instruction which has a different working frame.

[Levi-Armstrong]

Yes, take a look at the puzzle piece example. This accomplished by defining a profile which gets assigned to the move instruction. Each move instruction has profile and path profile key. The profile key only applies to the waypoint associated with move instruction waypoint and the path profile key apply any point interpolated between this instruction and the previous. Depending one which planner or pipeline you will need to define a profile for each. In the case of using descartes this would be defining a waypoint sampler which generates a set of cartesian pose sampling at some resolution around the z-axis. In the case of trajopt you just need to set the 5th coefficient to zero.

This file shows examples for each of the planners creating custom profiles for tool z-axis free and then how you added them to the planning server and uses them when creating the program.

[naren-sriram-robo]

Thank you so much. This was really the last thing I wanted and we got it to work for our scenario. We had earlier implemented something similar to Descartes but I think TrajOpt does the job for us. Once again, thank you for being so prompt with responses.

Just a last follow up question if you do not mind - Where can I check the total cost of each trajectory and for each term info if possible? We need this data for analytics purpose. I was able to get the planning tome by manually adding a clock but total cost and cost per term info would be of great help too.
Thank you!

[Levi-Armstrong]

I don't believe it is exposed in a ros interface, but I would take a look this print function which shows how to extract the costs.

[naren-sriram-robo]

Okay sure. Thank you. That helped.
Just a question out of curiosity: If we want to add tolerance to cartesian constraint, how do you figure we approach the problem? My team and I just took the function 'calcRotationalError()' from utils.cpp and changes the error values to be zero if they are within the tolerance, but that does not seem to work. Do you have any approach in mind or is our understanding of how this error works, wrong?

[Levi-Armstrong]

Yea, that probably does not work because the jacobian calculator is not provided. You will want to provide a jocobian calculator which use the error function which does not consider tolerances. This way you always have a valid jacobian. The reason it probably is not working well is because the jacobian is all zeros when you are within tolerance which is not correct.