From 212eb012490cc1761230a1dbea6aa5c6db51f2da Mon Sep 17 00:00:00 2001 From: mohamedsayed18 Date: Thu, 10 Jun 2021 14:51:40 +0300 Subject: [PATCH 1/4] replace joints with Links --- _source/intro/tesseract_setup_wizard_doc.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/_source/intro/tesseract_setup_wizard_doc.rst b/_source/intro/tesseract_setup_wizard_doc.rst index 008a438ca..47294240a 100644 --- a/_source/intro/tesseract_setup_wizard_doc.rst +++ b/_source/intro/tesseract_setup_wizard_doc.rst @@ -131,7 +131,7 @@ Specifying a chain is the most common way to specify kinematics group. 6. Click **ADD GROUP** to add the chain to your list of kinematics groups. -Method 2: Select All Links +Method 2: Select All Joints -------------------------- You can also specify a kinematics group by specifying each joint in the group. To do this: @@ -150,7 +150,7 @@ You can also specify a kinematics group by specifying each joint in the group. T 7. Once you have added all joints to your group, click **ADD GROUP**. -Method 3: Select All Joints +Method 3: Select All Links --------------------------- You can also specify a kinematics group by specifying each link in the group. To do this: From 75eac82986b7f6f7b2ac51929c50dacb5ce3f6b8 Mon Sep 17 00:00:00 2001 From: mohamedsayed18 Date: Thu, 10 Jun 2021 19:10:10 +0300 Subject: [PATCH 2/4] fix the number of dashes --- _source/intro/tesseract_setup_wizard_doc.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/_source/intro/tesseract_setup_wizard_doc.rst b/_source/intro/tesseract_setup_wizard_doc.rst index 47294240a..d2cf99ba6 100644 --- a/_source/intro/tesseract_setup_wizard_doc.rst +++ b/_source/intro/tesseract_setup_wizard_doc.rst @@ -132,7 +132,7 @@ Specifying a chain is the most common way to specify kinematics group. 6. Click **ADD GROUP** to add the chain to your list of kinematics groups. Method 2: Select All Joints --------------------------- +--------------------------- You can also specify a kinematics group by specifying each joint in the group. To do this: @@ -151,7 +151,7 @@ You can also specify a kinematics group by specifying each joint in the group. T 7. Once you have added all joints to your group, click **ADD GROUP**. Method 3: Select All Links ---------------------------- +-------------------------- You can also specify a kinematics group by specifying each link in the group. To do this: From 2ae31eaec454e9b8b3c61b9c37ea3b6251dbabdb Mon Sep 17 00:00:00 2001 From: mohamedsayed18 Date: Thu, 15 Jul 2021 16:19:33 +0300 Subject: [PATCH 3/4] glass_upright readme --- examples_readme/glass_upright.md | 151 +++++++++++++++++++++++++++++++ 1 file changed, 151 insertions(+) create mode 100644 examples_readme/glass_upright.md diff --git a/examples_readme/glass_upright.md b/examples_readme/glass_upright.md new file mode 100644 index 000000000..1fb8e7f08 --- /dev/null +++ b/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 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(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(); +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("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. \ No newline at end of file From c20fec04a3c970ae4ab5d2256dc4299e999d20fb Mon Sep 17 00:00:00 2001 From: mohamedsayed18 Date: Thu, 15 Jul 2021 16:48:27 +0300 Subject: [PATCH 4/4] Puzzle auxiliray readme --- examples_readme/puzzle_aux.md | 129 ++++++++++++++++++++++++++++++++++ 1 file changed, 129 insertions(+) create mode 100644 examples_readme/puzzle_aux.md diff --git a/examples_readme/puzzle_aux.md b/examples_readme/puzzle_aux.md new file mode 100644 index 000000000..2d45605ce --- /dev/null +++ b/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(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(); +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(); +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(); +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("CARTESIAN", trajopt_plan_profile); +planning_server.getProfiles()->addProfile("DEFAULT", + trajopt_composite_profile); +planning_server.getProfiles()->addProfile("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.