Open thume4zzzz opened 12 months ago
In the code snippet from this example, we are configuring a TrajOpt profile that is specific to this application under the name cartesian_program
, and it has different values than the default values of the TrajOptDefaultCompositeProfile
class. Therefore, when you change the composite instruction to use the DEFAULT
profile, you're telling the planner to apply various costs and constraints that may not really be appropriate
If you could post the full TrajOpt log from the last iteration of the optimization, we may be able to identify what costs/constraints were causing the problem
In the code snippet from this example, we are configuring a TrajOpt profile that is specific to this application under the name
cartesian_program
, and it has different values than the default values of theTrajOptDefaultCompositeProfile
class. Therefore, when you change the composite instruction to use theDEFAULT
profile, you're telling the planner to apply various costs and constraints that may not really be appropriateIf you could post the full TrajOpt log from the last iteration of the optimization, we may be able to identify what costs/constraints were causing the problem
Thank you for your reply. I create a custom cartesian_program just like the example, now it works well. Here is another question. I wrote a test program that commands the manipulator to follow a circular path in Cartesian space.
int point_num = 11;
Eigen::Vector3d center(x,y,z);
for (std::size_t i = 0; i < point_num; ++i)
{
theta = 2*M_PI / (point_num - 1) * i ;
Eigen::Vector3d end(x-r*sin(theta), y, z+r*cos(theta));
Eigen::Vector3d norm_z = (end - center).normalized();
Eigen::Vector3d x_axis(0, 1, 0);
Eigen::Vector3d y_axis = (norm_z.cross(x_axis)).normalized();
Eigen::Isometry3d pose;
pose.matrix().col(0).head<3>() = x_axis;
pose.matrix().col(1).head<3>() = y_axis;
pose.matrix().col(2).head<3>() = norm_z;
pose.matrix().col(3).head<3>() = end;
CartesianWaypointPoly wp{ CartesianWaypoint(pose) };
MoveInstruction plan_instruction(wp, MoveInstructionType::FREESPACE, "freespace_profile");
plan_instruction.setDescription("waypoint_" + std::to_string(i));
program.appendMoveInstruction(plan_instruction);
}
The planner can correctly solve the trajectory. However, if I change the point_num to 10 or 20, the planner will fail to find a solution. What is the problem?
Here is the TrajOpt log from the last iteration of the optimization. The cartesian poses of planned trajectory deviate greatly from the set values. log.txt
Here is the TrajOpt log from the last iteration of the optimization. The cartesian poses of planned trajectory deviate greatly from the set values.
A few questions:
basic_cartesian_example.cpp
?trajopt
repo are you on?
The optimization seems to have failed because all of the cartesian position costs (CartPos_JointPosition
in the new_exact
column) are very large but should be zero.
My guess as to why this fails is that you might not have given TrajOpt a good joint seed for those Cartesian positions, so it got stuck and wasn't able to find a way to meet those constraints. I would recommend adding a SimplePlanner
step before going to TrajOpt. This planner will solve IK for each Cartesian waypoint and set that IK solution as the seed for TrajOpt. Then TrajOpt can optimize that IK solution (if your robot has more than 6 DoF) and will likely be more successful
Here is the TrajOpt log from the last iteration of the optimization. The cartesian poses of planned trajectory deviate greatly from the set values.
A few questions:
* Is this log from your custom application you described above or from `basic_cartesian_example.cpp`? * Are you using the TrajOpt IFOPT planner or the "regular" TrajOpt planner? What version of the `trajopt` repo are you on? * I have only used the "regular" TrajOpt planner before, and I have not seen it create joint velocity costs for every set of decision variables as in your log; I also don't recognize the names of the costs and constraints
The optimization seems to have failed because all of the cartesian position costs (
CartPos_JointPosition
in thenew_exact
column) are very large but should be zero.My guess as to why this fails is that you might not have given TrajOpt a good joint seed for those Cartesian positions, so it got stuck and wasn't able to find a way to meet those constraints. I would recommend adding a
SimplePlanner
step before going to TrajOpt. This planner will solve IK for each Cartesian waypoint and set that IK solution as the seed for TrajOpt. Then TrajOpt can optimize that IK solution (if your robot has more than 6 DoF) and will likely be more successful
Could you please give an example how to add a SimplePlanner?
Here is the TrajOpt log from the last iteration of the optimization. The cartesian poses of planned trajectory deviate greatly from the set values.
A few questions:
* Is this log from your custom application you described above or from `basic_cartesian_example.cpp`? * Are you using the TrajOpt IFOPT planner or the "regular" TrajOpt planner? What version of the `trajopt` repo are you on? * I have only used the "regular" TrajOpt planner before, and I have not seen it create joint velocity costs for every set of decision variables as in your log; I also don't recognize the names of the costs and constraints
The optimization seems to have failed because all of the cartesian position costs (
CartPos_JointPosition
in thenew_exact
column) are very large but should be zero. My guess as to why this fails is that you might not have given TrajOpt a good joint seed for those Cartesian positions, so it got stuck and wasn't able to find a way to meet those constraints. I would recommend adding aSimplePlanner
step before going to TrajOpt. This planner will solve IK for each Cartesian waypoint and set that IK solution as the seed for TrajOpt. Then TrajOpt can optimize that IK solution (if your robot has more than 6 DoF) and will likely be more successful* This log is from my custom application based on basic_cartesian_example.cpp (use the same cartesian_program). * I'm using TrajOpt IFOPT and the latest version trajopt repo (0.6.1).
Could you please give an example how to add a SimplePlanner?
Update: I follow the example in chain_example.cpp which utilizes descartes to get the seed for trajopt. But descartes fails to get monitor environment information. It seems like that ik solver (kdl is used) fails to get any available solution in some waypoints, but the given waypoints are definitely reachable. descartes_log.txt My robot is redundant so opw ik solver is not available.
If your robot is redundant and you are not discretely sampling the extra DoF, the descartes
planner is not going to provide any additional benefit over the simple planner (i.e., interpolation and IK solving). If you notice in the descartes
log, no waypoint (rung) in your plan has more than one IK solution (node), meaning that if all your waypoints were feasible there would only be one path through the graph and it would be equivalent to sequentially solving IK.
It appears the [example is using a pipeline that only includes TrajOpt IFOPT](). To use the simple planner in conjunction with TrajOpt IFOPT, you'll have to make a custom pipeline that is similar to this one. It would probably look like this:
TrajOptIfoptTask:
class: PipelineTaskFactory
config:
conditional: true
inputs: [input_data]
outputs: [output_data]
nodes:
DoneTask:
class: DoneTaskFactory
config:
conditional: false
ErrorTask:
class: ErrorTaskFactory
config:
conditional: false
MinLengthTask:
class: MinLengthTaskFactory
config:
conditional: true
inputs: [input_data]
outputs: [output_data]
+ SimpleMotionPlannerTask:
+ class: SimpleMotionPlannerTaskFactory
+ config:
+ conditional: true
+ inputs: [input_data]
+ outputs: [output_data]
+ format_result_as_input: true
TrajOptIfoptMotionPlannerTask:
class: TrajOptIfoptMotionPlannerTaskFactory
config:
conditional: true
inputs: [output_data]
outputs: [output_data]
format_result_as_input: false
DiscreteContactCheckTask:
class: DiscreteContactCheckTaskFactory
config:
conditional: true
inputs: [output_data]
IterativeSplineParameterizationTask:
class: IterativeSplineParameterizationTaskFactory
config:
conditional: true
inputs: [output_data]
outputs: [output_data]
edges:
+ - source: SimpleMotionPlannerTask
+ destinations: [ErrorTask, MinLengthTask]
- source: MinLengthTask
destinations: [ErrorTask, TrajOptIfoptMotionPlannerTask]
- source: TrajOptIfoptMotionPlannerTask
destinations: [ErrorTask, DiscreteContactCheckTask]
- source: DiscreteContactCheckTask
destinations: [ErrorTask, IterativeSplineParameterizationTask]
- source: IterativeSplineParameterizationTask
destinations: [ErrorTask, DoneTask]
terminals: [ErrorTask, DoneTask]
The simple planner has a few profiles that determine how it performs interpolation. Either the LVS (default) or fixed size plan profiles should work here
Thank you for replying. I have modified the yaml file according to your advice. But how to add a simple planner in the application? I use the following code :
auto profiles = std::make_shared<ProfileDictionary>();
// Simple planner profiles
auto simple_planner_profile = std::make_shared<SimplePlannerLVSPlanProfile>();
profiles->addProfile<SimplePlannerPlanProfile>(SIMPLE_DEFAULT_NAMESPACE, "DEFAULT", simple_planner_profile);
// Trajopt profiles
auto composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
composite_profile->collision_cost_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
composite_profile->collision_constraint_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE;
composite_profile->smooth_velocities = true;
composite_profile->smooth_accelerations = false;
composite_profile->smooth_jerks = false;
composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
profiles->addProfile<TrajOptIfoptCompositeProfile>(
TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile);
auto plan_profile = std::make_shared<TrajOptIfoptDefaultPlanProfile>();
plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6);
plan_profile->joint_coeff = Eigen::VectorXd::Ones(numJoints);
profiles->addProfile<TrajOptIfoptPlanProfile>(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile);
profiles->addProfile<TrajOptIfoptPlanProfile>(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile);
// Create task
const std::string task_name = "TrajOptIfoptTask";
TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name);
const std::string output_key = task->getOutputKeys().front();
// Create Task Composer Problem
auto problem = std::make_unique<PlanningTaskComposerProblem>(env_, profiles);
problem->input_instruction = program;
but get Error: Input instructions to MotionPlannerTask: SimpleMotionPlannerTask must be a composite instruction
But how to add a simple planner in the application?
The simple planner is already a part of the TrajOptIfoptTask
"pipeline", so it get's added as a part of TaskComposerNode::UPtr task = factory.createTaskComposerNode("TrajOptIfoptTask");
.
Error: Input instructions to MotionPlannerTask: SimpleMotionPlannerTask must be a composite instruction
This is exactly the problem described in #399. It appears that your problem is set up incorrectly. Per the discussion on that issue, the better way to solve this issue is to add a ProcessInputTask
which looks at problem->input_instruction
and makes sure that the information gets set in the correct place for the first task in the underlying DataStorage
object:
TrajOptIfoptTask:
class: PipelineTaskFactory
config:
conditional: true
inputs: [input_data]
outputs: [output_data]
nodes:
DoneTask:
class: DoneTaskFactory
config:
conditional: false
ErrorTask:
class: ErrorTaskFactory
config:
conditional: false
+ ProcessInputTask:
+ class: ProcessPlanningInputTaskFactory
+ config:
+ conditional: false
+ outputs: [input_data]
MinLengthTask:
class: MinLengthTaskFactory
config:
conditional: true
inputs: [input_data]
outputs: [output_data]
+ SimpleMotionPlannerTask:
+ class: SimpleMotionPlannerTaskFactory
+ config:
+ conditional: true
+ inputs: [input_data]
+ outputs: [output_data]
+ format_result_as_input: true
TrajOptIfoptMotionPlannerTask:
class: TrajOptIfoptMotionPlannerTaskFactory
config:
conditional: true
inputs: [output_data]
outputs: [output_data]
format_result_as_input: false
DiscreteContactCheckTask:
class: DiscreteContactCheckTaskFactory
config:
conditional: true
inputs: [output_data]
IterativeSplineParameterizationTask:
class: IterativeSplineParameterizationTaskFactory
config:
conditional: true
inputs: [output_data]
outputs: [output_data]
edges:
+ - source: ProcessInputTask
+ - destinations: [SimpleMotionPlannerTask]
+ - source: SimpleMotionPlannerTask
+ destinations: [ErrorTask, MinLengthTask]
- source: MinLengthTask
destinations: [ErrorTask, TrajOptIfoptMotionPlannerTask]
- source: TrajOptIfoptMotionPlannerTask
destinations: [ErrorTask, DiscreteContactCheckTask]
- source: DiscreteContactCheckTask
destinations: [ErrorTask, IterativeSplineParameterizationTask]
- source: IterativeSplineParameterizationTask
destinations: [ErrorTask, DoneTask]
terminals: [ErrorTask, DoneTask]
It seems like that KDL fails to generate joint solution for a reachable cartesian pose. Is it possible to switch to a different ik solver?
It seems like that KDL fails to generate joint solution for a reachable cartesian pose. Is it possible to switch to a different ik solver?
Depends on your robot. If it is a 6-DOF spherical wrist robot, you could use the OPW solver. If it's a UR, you can use the UR solvers. Otherwise, the KDL solvers are really the only option at the moment. You could update the seed state in your IK call to be closer to the final solution; that typically helps a lot with making the solver converge
Thank you for you reply. Finally I figure it out by changing the seed for KDL in each IK call. I also turn off collision check in Descartes. Now Descartes + trajopt work well when
trajopt_composite_profile->collision_cost_config.enabled = false;
But if I turn the collision cost on, trajopt will get stuck in the first Iteration. It seems like it takes much time to convexify and process collision constraints. Shall I do VHACD decomposition and generate convex hulls for collision objects first?
Hi, I'm using trajopt to plan a trajectory subject to Cartesian waypoint constraints. If I use the following codes to create the planning program, everything works fine.
However, if I follow the example in _basic_cartesianexample.cpp and use:
the planner will fail because some of the waypoint constraints can not be satisfied.
Could someone tell me what is the difference?