What is the cartesian_coeff value for TrajOptDefaultPlanProfile mean?

[mohamedsayed18]

Hello everyone,
I'm working on an example to build a collabration between a robot arm and an external axis. I checked the puzzle_piece_auxillary_axes_example.launch as it is similar to what I'm working on.
There is some parts I don't understand
what size and what values should be given to cartesian_coeff

// Create TrajOpt Profile
auto trajopt_plan_profile = std::make_shared<tesseract_planning::TrajOptDefaultPlanProfile>();
trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5);
trajopt_plan_profile->cartesian_coeff(3) = 2;
trajopt_plan_profile->cartesian_coeff(4) = 2;
trajopt_plan_profile->cartesian_coeff(5) = 0;    

Is this related to add constraints? in my example I'd like to keep the endeffector pointing down.
Thanks for your work

[Levi-Armstrong]

The size of cartesian_coeff should be six. The first three correspond to translation and the last three to rotation. The trajopt_plan_profile->cartesian_coeff(5) = 0; indicates it is free to rotate around the z-axis. I the value is greater than zero it is considered constrained and the coefficient represents a weight/scale applied to the error and gradient.

[mohamedsayed18]

The first three correspond to translation and the last three to rotation

Is this related to the end effector? For example, if I don't want the end effector to do any rotation, just translation movements. I should set it like this

trajopt_plan_profile->cartesian_coeff(3) = 200;
trajopt_plan_profile->cartesian_coeff(4) = 200;
trajopt_plan_profile->cartesian_coeff(5) = 200;

[mpowelson]

The plan profile governs PlanInstructions. So this would be used to say that a specific waypoint not rotate but may translate. However, the timesteps in between those waypoints will still be free to rotate. You could cheat the system by making the moves LINEAR instead of FREESPACE in the PlanInstruction, but that will obvious make the moves linear. You could also just make sure that there is exactly one timestep per PlanInstruction by modifying the inputs to the SimplePlanner. That is what the glass upright example in tesseract_ros is doing.

The ideal way would be to add a CartesianVelocity constraint via the composite profile. However, that is not currently implemented, and I don't know what the status of the trajopt CartVelErrCalculator is.

[mohamedsayed18]

In glass_upright_example I noticed that when the cartesian coefficient is to a high value for the translation

trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5);
trajopt_plan_profile->cartesian_coeff(0) = 200;
trajopt_plan_profile->cartesian_coeff(1) = 200;
trajopt_plan_profile->cartesian_coeff(2) = 200;

The robot's path goes through the sphere, didn't avoid it.
Does this mean that it tries to find the path to the goal whatever the cost is?
If I used TrajOptTaskflow with collision checking, it can avoid such a path that passes through obstacles?

[Levi-Armstrong]

By setting the cartesian coeff to anything other than zero restricts it to go to the cartesian position. In the case of the glass upright, it is a straight line from the start pose to the end pose which goes right through the sphere. In this case it was instructed to follow the line but also avoid a collision but because the coeff for the cartesian position is significantly higher than the one for the collision it dominates the optimization. What are you wanting to accomplish by enabling the cartesian coefficient?

[Levi-Armstrong]

On a side note setting the coefficients very high can cause solver failures. Every constraint or cost gets converted to a quadratic function e(x) = A + Bx + Cx^2 and the coefficient is applied as follows e(x) = WA + (WB)x + (W*C)x^2. Usually, recommend setting the weight/coeff between 1-20, but can go higher but if you start seeing solver failures you probably have gone too high.