examples_readme

examples_readme/glass_upright.md

@@ -0,0 +1,151 @@
+# Glass Upright example
+
+## Code
+
+```cpp
+tesseract_environment::Command::Ptr GlassUprightExample::addSphere()
+```
+
+This method adds sphere to the environment
+
+```cpp
+bool GlassUprightExample::run()
+```
+
+The run method is invoked inside the main node, we will explain what this method is doing in detail.
+
+```cpp
+// Set the robot initial state
+std::vector<std::string> joint_names;
+joint_names.push_back("joint_a1");
+joint_names.push_back("joint_a2");
+joint_names.push_back("joint_a3");
+joint_names.push_back("joint_a4");
+joint_names.push_back("joint_a5");
+joint_names.push_back("joint_a6");
+joint_names.push_back("joint_a7");
+
+Eigen::VectorXd joint_start_pos(7);
+joint_start_pos(0) = -0.4;
+joint_start_pos(1) = 0.2762;
+joint_start_pos(2) = 0.0;
+joint_start_pos(3) = -1.3348;
+joint_start_pos(4) = 0.0;
+joint_start_pos(5) = 1.4959;
+joint_start_pos(6) = 0.0;
+
+Eigen::VectorXd joint_end_pos(7);
+joint_end_pos(0) = 0.4;
+joint_end_pos(1) = 0.2762;
+joint_end_pos(2) = 0.0;
+joint_end_pos(3) = -1.3348;
+joint_end_pos(4) = 0.0;
+joint_end_pos(5) = 1.4959;
+joint_end_pos(6) = 0.0;
+
+env_->setState(joint_names, joint_start_pos);
+```
+
+The robot initial state is set by defining the `joint_names` which is a vector carry the joint names as defined in the robot description.
+`joint_start_pos` and `joint_end_pos` are vectors carry the values for the joints defined in the `Joint_names`.
+
+The values of the `joint_start_pos` is set as the start state of the robot using the `setState` method.
+
+```cpp
+// Create Program
+CompositeInstruction program("FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator"));
+```
+
+Here we created the program which is a group of instruction that will be added in the next sections.
+The program takes three parameters. First is a string which is a key or a label for the program.
+Second is the `CompositeInstructionOrder` it can be one of the following three values:
+
+* ORDERED: Must go in forward
+* UNORDERED: Any order is allowed
+* ORDERED_AND_REVERABLE: An go forward or reverse the order
+
+The third parameter is the `ManipulatorInfo` which here is initialized by the `manipulator` which should match the name of the group in srdf file.
+
+Next we will add the instructions to our program. To create an instruction we need to have an a waypoint which is the point we would like to go to by this instruction.
+
+```cpp
+  // Start and End Joint Position for the program
+  Waypoint wp0 = StateWaypoint(joint_names, joint_start_pos);
+  Waypoint wp1 = StateWaypoint(joint_names, joint_end_pos);
+```
+
+Two waypoints were defined using the `StateWaypoint` method, which takes the names and the desired values of the joints.
+
+```cpp
+PlanInstruction start_instruction(wp0, PlanInstructionType::START);
+program.setStartInstruction(start_instruction);
+```
+
+A plan instruction is created which takes the first waypoint `wp0`, set the type of the instruction as `START`.
+
+The PlanInstructionType can be one of the following:
+
+* LINEAR: Straight line motion to the waypoint
+* FREESPACE: Free motion, similar to the point to point
+* CIRCULAR: Circular motion
+* START: Set the point as the start point
+
+Then we added this point to the program as the start instruction using `setStartInstruction` method.
+
+```cpp
+// Assign a linear motion so cartesian is defined as the target
+PlanInstruction plan_f0(wp1, PlanInstructionType::LINEAR, "UPRIGHT");
+plan_f0.setDescription("freespace_plan");
+// Add Instructions to program
+program.push_back(plan_f0);
+```
+
+The second plan instruction is a linear motion to the second waypoint, Then we push back the point to the program.
+
+```cpp
+// Create Process Planning Server
+ProcessPlanningServer planning_server(std::make_shared<ROSProcessEnvironmentCache>(monitor_), 5);
+planning_server.loadDefaultProcessPlanners();
+```
+
+Then we create the ProcessPlanningServer which will take the program as a request and return the output trajectory as a response.
+
+`loadDefaultProcessPlanners`: loads the default planners.
+
+We can add some planning profiles to our planning server, each profile add some costs and constrains to our problem
+
+```cpp
+// 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(0) = 0;
+trajopt_plan_profile->cartesian_coeff(1) = 0;
+trajopt_plan_profile->cartesian_coeff(2) = 0;
+```
+
+One of the parameters we can control using the `TrajOptDefaultPlanProfile` is the `cartesian_coeff`.
+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(0) = 0;` indicates it is free to translate along the x-axis.
+If the value is greater than zero it is considered constrained and the coefficient represents a weight/scale applied to the error and gradient.
+
+```cpp
+// Add profile to Dictionary
+planning_server.getProfiles()->addProfile<tesseract_planning::TrajOptPlanProfile>("UPRIGHT", trajopt_plan_profile);
+```
+
+We add the profile to the planning server
+
+```
+// Create Process Planning Request
+ProcessPlanningRequest request;
+request.name = tesseract_planning::process_planner_names::TRAJOPT_PLANNER_NAME;
+request.instructions = Instruction(program);
+
+request.instructions.print("Program: ");
+
+// Solve process plan
+ProcessPlanningFuture response = planning_server.run(request);
+planning_server.waitForAll();
+```
+
+We create the request and pass it to the run method to get the response.

examples_readme/puzzle_aux.md

@@ -0,0 +1,129 @@
+# Puzzle Piece auxiliary axes example
+
+The puzzle piece examples show a small collaborative robot manipulating a puzzle piece to debur the edges with a grinder which has two axes of rotation.
+
+![Puzzle piece](https://github.com/ros-industrial-consortium/trajopt_ros/blob/master/gh_pages/_static/example_gifs/puzzle_piece_with_positioner.gif)
+
+```cpp
+tesseract_common::VectorIsometry3d tool_poses = makePuzzleToolPoses();
+```
+
+`makePuzzleToolPoses`: reads the target poses of the puzzle piece from the
+`/config/puzzle_bent.csv` and return a list of the target poses.
+
+```cpp
+// Create cartesian waypoint
+Waypoint wp = CartesianWaypoint(tool_poses[0]);
+PlanInstruction plan_instruction(wp, PlanInstructionType::START, "CARTESIAN");
+plan_instruction.setDescription("from_start_plan");
+program.setStartInstruction(plan_instruction);
+```
+
+The first pose is set as the start poses.
+
+```cpp
+for (std::size_t i = 1; i < tool_poses.size(); ++i)
+{
+    Waypoint wp = CartesianWaypoint(tool_poses[i]);
+    PlanInstruction plan_instruction(wp, PlanInstructionType::LINEAR, "CARTESIAN");
+    plan_instruction.setDescription("waypoint_" + std::to_string(i));
+    program.push_back(plan_instruction);
+}
+```
+
+A linear motion is set between the rest of the points.
+
+```cpp
+const std::string new_planner_name = "TRAJOPT_NO_POST_CHECK";
+tesseract_planning::TrajOptTaskflowParams params;
+params.enable_post_contact_discrete_check = false;
+params.enable_post_contact_continuous_check = false;
+planning_server.registerProcessPlanner(new_planner_name,
+                                        std::make_unique<tesseract_planning::TrajOptTaskflow>(params));
+```
+
+A trajopt taskflow is used without collision checking by setting
+`enable_post_contact_discrete_check` & `enable_post_contact_continuous_check` to `false`.
+
+Next we will create TrajOpt Profiles.
+
+```cpp
+// 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;
+```
+
+In the `TrajOptDefaultPlanProfile`, the `cartesian_coeff` is a vector of size 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.
+If the value is greater than zero it is considered constrained and the coefficient represents a
+weight/scale applied to the error and gradient.
+
+```cpp
+auto trajopt_composite_profile = std::make_shared<tesseract_planning::TrajOptDefaultCompositeProfile>();
+trajopt_composite_profile->collision_constraint_config.enabled = false;
+trajopt_composite_profile->collision_cost_config.enabled = true;
+trajopt_composite_profile->collision_cost_config.safety_margin = 0.025;
+trajopt_composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::SINGLE_TIMESTEP;
+trajopt_composite_profile->collision_cost_config.coeff = 1;
+```
+
+In the `TrajOptDefaultCompositeProfile`, we can set the following parameters:
+
+`collision_constraint_config`: to decide to apply constraints on the collision
+
+`collision_cost_config`: to give costs for the collision.
+
+`collision_cost_config.safety_margin`: define the safety margin for collision
+
+`collision_cost_config.type`: It can take the following values:
+
+* SINGLE_TIMESTEP: TODO, description.
+* DISCRETE_CONTINUOUS: TODO, description.
+* CAST_CONTINUOUS: TODO, description.
+
+```cpp
+auto trajopt_solver_profile = std::make_shared<tesseract_planning::TrajOptDefaultSolverProfile>();
+trajopt_solver_profile->convex_solver = sco::ModelType::OSQP;
+trajopt_solver_profile->opt_info.max_iter = 200;
+trajopt_solver_profile->opt_info.min_approx_improve = 1e-3;
+trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
+```
+
+In `TrajOptDefaultSolverProfile` we specify the type of the convex solver to use it can be:
+GUROBI, OSQP, QPOASES, BPMPD, AUTO_SOLVER
+
+`max_iter`: the maximum number of iterations to find a solution.
+
+`min_approx_improve`: the minimum improvement that for the solution after each iteration.
+
+`min_trust_box_size`: TODO, describe
+
+```cpp
+// Add profile to Dictionary
+planning_server.getProfiles()->addProfile<tesseract_planning::TrajOptPlanProfile>("CARTESIAN",          trajopt_plan_profile);
+planning_server.getProfiles()->addProfile<tesseract_planning::TrajOptCompositeProfile>("DEFAULT",
+                                                                                        trajopt_composite_profile);
+planning_server.getProfiles()->addProfile<tesseract_planning::TrajOptSolverProfile>("DEFAULT",
+                                                                                    trajopt_solver_profile);
+```
+
+We add the profiles created above to our planning server.
+
+```cpp
+// Create Process Planning Request
+ProcessPlanningRequest request;
+request.name = new_planner_name;
+request.instructions = Instruction(program);
+```
+
+We create a request object and add the program to its instruction.
+
+```cpp
+ProcessPlanningFuture response = planning_server.run(request);
+```
+
+we use the run method providing the request as a parameter to get the response.