Working Trajopt #1

ommmid · 2019-07-23T02:19:24Z

The tesseract env and kin are not used at all. They are replaced with what we have in planning_scene, for that I copied and adjusted the needed files from trajopt_ros, i.e., problem_description and kinematic_terms
The constraint from planning interface request is converted to the constraint we need in trajopt and that is only for joint state constraint for now.
No response from tesseract is used. planning interface response is created and fed internally.

Problems:

The start state for trajopt is not set, that is probably why I get a solution that has the same joint values for all timesteps except the last timestep
start_fixed is set to false. if it is true it means: it constraints the joint values to be their initialized ones but because the start state is not set, then it creates problem. I am not sure about this.
Please ignore the followings:
There are few struct and classes that are not used
There are headers that are included in different files that might not be used.
Also in CMakeLists, there might be extra or unused packages.

@davetcoleman @BryceStevenWilley @henningkayser @felixvd @j-petit

BryceStevenWilley

Just some initial thoughts, most comments you can ignore until this is ready for a full review, but most of the comments on the TrajOpt interface document are important.

moveit_planners/trajopt/trajopt_interface_plugin_description.xml

moveit_planners/trajopt/launch/gdb_settings.gdb

moveit_planners/trajopt/package.xml

moveit_planners/trajopt/src/kinematic_terms.cpp

BryceStevenWilley · 2019-07-23T03:19:28Z

moveit_planners/trajopt/include/kinematic_terms.h

+  CartPoseErrCalculator(const Eigen::Isometry3d& pose,
+                        const planning_scene::PlanningSceneConstPtr planning_scene,
+                        std::string link,
+                        Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity())


tcp is usused, should be added to the target_pose_inv * iteration_new_pose expression below.

I am not sure about using tcp as I am using getGlobalLinkTransform to get the global transform of the end-effector. I will investigate this more when I try to add Cartesian pose constraint

moveit_planners/trajopt/src/trajopt_interface.cpp

BryceStevenWilley · 2019-07-23T13:58:22Z

moveit_planners/trajopt/src/trajopt_interface.cpp

+      // Create place pose constraint
+      std::shared_ptr<trajopt_interface::JointPosTermInfo> joint_pos(new trajopt_interface::JointPosTermInfo);
+      joint_pos->term_type = trajopt::TT_CNT;
+      joint_pos->name = "joint pose name ^_^";


moveit_planners/trajopt/src/trajopt_interface.cpp

BryceStevenWilley · 2019-07-23T13:59:51Z

moveit_planners/trajopt/src/trajopt_interface.cpp

+    moveit::core::RobotState start_state(planning_scene->getCurrentState());
+    moveit::core::robotStateMsgToRobotState(req.start_state, start_state);
+    // This function converts the moveit_msgs::RobotState (from req.start_state) to moveit::core::RobotState (start_state).
+    start_state.update();


Your PR comment mentions that the start state is not set. How is anything working if you don't set the start state? If you are only setting the goals state, then none of the trajectories you get should be feasible.

I meant the start state as a constraint for trajopt as we talked in the previous meeting.

Ah, I see. I think I misunderstood what you meant by 'somewhat working': I assumed that you had gotten valid trajectories on some examples and had fixed the issue we talked about on Thursday.

I added the the start state as a constraint too.

davetcoleman · 2019-07-23T16:34:55Z

start_fixed is set to false. if it is true it means: it constraints the joint values to be their initialized ones but because the start state is not set, then it creates problem. I am not sure about this.

This seems like a really important detail to understand. Have you been able to read through its behavior in Tesseract to understand what it does? Maybe @Levi-Armstrong could quickly shed some light on this variable

davetcoleman

Can you share relevant diagrams explaining the relationship of the files / classes in this PR?

davetcoleman · 2019-07-23T18:17:28Z

moveit_planners/trajopt/config/setup.yaml

@@ -0,0 +1,14 @@
+trajopt_param:


please add comments explaining what each of these parameters do, so that reviewers can better understand this PR

You may want to cite the publication / note the paper's notation in the comments

yaml file is the right place to put the citation or it should be in another file?

sorry, i didn't mean cite the paper, i meant explain how these parameters relate to what were presented in the original research, since the var names probably differ

davetcoleman · 2019-07-23T18:19:12Z

moveit_planners/trajopt/include/kinematic_terms.h

+ * @brief Used to calculate the error for StaticCartPoseTermInfo
+ * This is converted to a cost or constraint using TrajOptCostFromErrFunc or TrajOptConstraintFromErrFunc
+ */
+struct CartPoseErrCalculator : public sco::VectorOfVector


Have you used rviz_visual_tools to better understand if your vectors are in the right frame, and if the error calculation intuitively makes sense?

I've never seen a struct inheritance... this isn't normal

I got it from the original trajopt pakage. Should I change them to class?

I am actually using rviz_visual_tools

yes, per Google style, if it has functions, it should be a class

moveit_planners/trajopt/include/problem_description.h

moveit_planners/trajopt/include/trajopt_interface.h

moveit_planners/trajopt/src/kinematic_terms.cpp

moveit_planners/trajopt/src/test_trajopt.cpp

moveit_setup_assistant/src/widgets/configuration_files_widget.cpp

ommmid · 2019-07-23T19:18:15Z

start_fixed is set to false. if it is true it means: it constraints the joint values to be their initialized ones but because the start state is not set, then it creates problem. I am not sure about this.

This seems like a really important detail to understand. Have you been able to read through its behavior in Tesseract to understand what it does? Maybe @Levi-Armstrong could quickly shed some light on this variable

#1 (comment)

ommmid · 2019-07-23T20:29:59Z

Can you share relevant diagrams explaining the relationship of the files / classes in this PR?

There is a lot of infrastructure missing to get this to actually run... - the plugin needs to be registered - pr2_moveit_config is not in moveit_resources

* Clarify object importing * Add suggestion (LARGE_MESH_THRESHOLD) * Add file filter * Add tooltip, clarify names * Fix and clarify names in robot states tab * Add dialogue to Clear button in robot states

Co-authored-by: Henning Kayser <henningkayser@picknik.ai>

…ration (moveit#2185) MoveIt wrapper to Time Optimal Trajectory Generation downsamples trajectory, but its hard coded to keep waypoints with at least 0.001 radians of motion on a joint. For very long and/or dense input trajectories, the downsampled trajectory is still way too large for the iterative optimization algorithm to finish in reasonable time. I was seeing 3000 point trajectory taking ~15 seconds. This fix allows the user to set this downsampling density in minimum radians of motion between two waypoints to something else (presumably higher like 0.5 degrees [0.00875 radians]) in order to drastically improve the speed of this algorithm.

which (mostly) are redundant to the corresponding C++ tests.

- Remove "Import from File" and "Import from URL" buttons, but add those options as item in the shape combobox - Integrate MotionPlanningFrame::addObject() into MotionPlanningFrame::addSceneObject()

- grid layout for object position/orientation spin boxes (to not rely on chance for their column-wise layout anymore) - Limit position spin boxes to -999..999 This essentially cancels one digit to be displayed, which significantly reduces the minimum width of the widget.

clearSceneButtonClicked() -> clearScene() removeObjectButtonClicked() -> removeSceneObject() publishSceneButtonClicked()-> publishScene()

…is not set. Check this and warn the user if ref traj is empty trajectory_test not working yet

…o the test

Co-Authored-By: Ayush Garg <ayushgargdroid@gmail.com>

osqp_FOUND is false if osqp is pulled in trajopt_sco. osqp can not be found from the same CMakeLists that was used to pull down its repo and install it in build directory. A workaround is to install osqp from trajopt_utils and check if it exists in trajopt_sco. trajopt_sco is dependent on trajopt_utils Another fix: trajopt_problem_ from trajopt_interface needed to be corrected as a shared_pointer.

…jects. Contact type has a depth memeber but in tesseract, they use distance member in their ContactResult type

…calculate cc_nearest_points in MoveIt, Contact type does not have such a thing

…anceExpressions

… tesseract

…rect. Just used it with FCL and singletimestep not continous collision checking yet.

This version successfully uses Bullet collision detector from MoveIt to do the collsion checking and find singed distances. There are a lot of changes in different files but the most changes are done in collision_terms.cpp Things left to do: 1. This implementation is hard-coded. But it is easy to change this to a version that the user can enter a CollisionTermInfo. Currently this is done in trajopt_interface.cpp but it should be defined by user, its parametes should be added to setup.yaml as well. 2. It needs cleanup in the code base. 3. The PlanningScene that motion planners have acces to is a ConsPtr but we need to make changes in RobotState to be able to check collision. To this end, a child of the original PlanningScene is used through diff() function. For some reason, I can not use "checkCollision" function on this planning scene child. It turned out if I remove all the objects from the child and add them again, "checkCollision" operates correctly

BryceStevenWilley reviewed Jul 23, 2019

View reviewed changes

davetcoleman reviewed Jul 23, 2019

View reviewed changes

ommmid changed the title ~~Somewhat working Trajopt~~ Working Trajopt Jul 31, 2019

ommmid force-pushed the trajopt branch from c2f753f to 1918ccd Compare August 1, 2019 00:27

ommmid force-pushed the trajopt branch 4 times, most recently from d4013e1 to 71098bd Compare August 16, 2019 17:08

ommmid force-pushed the trajopt branch from 1f4417b to cc2497f Compare September 22, 2019 22:46

ommmid force-pushed the trajopt branch from 8da4bc0 to d175c48 Compare December 9, 2019 01:12

ommmid force-pushed the trajopt branch from cf43ed0 to 0339e80 Compare March 2, 2020 05:11

ommmid force-pushed the trajopt branch from 0339e80 to 391fb6a Compare March 24, 2020 00:11

v4hn and others added 15 commits July 2, 2020 13:29

partially transition unused test bin to rostest (moveit#2158)

349eb83

There is a lot of infrastructure missing to get this to actually run... - the plugin needs to be registered - pr2_moveit_config is not in moveit_resources

Clean up Rviz plugin, fix mesh scaling (moveit#2142)

5af7258

* Clarify object importing * Add suggestion (LARGE_MESH_THRESHOLD) * Add file filter * Add tooltip, clarify names * Fix and clarify names in robot states tab * Add dialogue to Clear button in robot states

Prefer vendor-specific OpenGL library

a617923

Co-authored-by: Henning Kayser <henningkayser@picknik.ai>

Fix a parameter mix-up in moveit_cpp loading (moveit#2187)

73fa9ea

moveit_servo: delete python integration tests (moveit#2186)

0274ea9

which (mostly) are redundant to the corresponding C++ tests.

use SYSTEM includes to suppress external warnings

93cdeab

rviz panel: widgets for adding geometric primitives

f21bcae

Fix segfault in PSM::clearOctomap() (moveit#2193)

538d15f

use moveit_resources for testing (moveit#2194)

d6666a9

Unify UI of adding primitive and mesh shapes

37b94e7

- Remove "Import from File" and "Import from URL" buttons, but add those options as item in the shape combobox - Integrate MotionPlanningFrame::addObject() into MotionPlanningFrame::addSceneObject()

Update query states if a collision object was attached/detached

16a7df1

Rename MotionPlanningFrame functions

157dd30

clearSceneButtonClicked() -> clearScene() removeObjectButtonClicked() -> removeSceneObject() publishSceneButtonClicked()-> publishScene()

Track local PS changes and publish them before planning

fe59e1d

omid and others added 29 commits February 13, 2021 10:42

fixed license

6160670

clang format

096a511

correctedd num_steps in the loop

6da7a17

When using motion planning display in rviz, the reference trajectory …

50b9b7b

…is not set. Check this and warn the user if ref traj is empty trajectory_test not working yet

run demo from panda_moveit_config befire running the test

b2622c2

Fixed the test. changed the type to interpolated.

c312079

clang format

8561c64

moved all the files from pand_mvoit_config and franka_description int…

8698f52

…o the test

Update moveit_planners/trajopt/trajopt/CMakeLists.txt

a01ce25

Co-Authored-By: Ayush Garg <ayushgargdroid@gmail.com>

Update moveit_planners/trajopt/trajopt/CMakeLists.txt

9ccfc5f

Co-Authored-By: Ayush Garg <ayushgargdroid@gmail.com>

Update moveit_planners/trajopt/trajopt/package.xml

748be0a

Co-Authored-By: Ayush Garg <ayushgargdroid@gmail.com>

rebased

df0fdc6

download osqp during configuration - from Jafar's PR

fae7dde

working on CollisionCost

08d177d

working on finding the right value for distances between collision ob…

3fb27ff

…jects. Contact type has a depth memeber but in tesseract, they use distance member in their ContactResult type

adjusted more parts of the code for MoveIt but what is left is how I …

9519236

…calculate cc_nearest_points in MoveIt, Contact type does not have such a thing

start adding collision temrs

30b4cb0

changed header extension

cf1a0af

adjusted CalcCollisions and CollisionstoDistance and CollisionsToDist…

b1fb905

…anceExpressions

no tesseract but the functionality should be tested

b3dab65

clean

0e167b1

replace GetKin() with GetPlanningScene

08fc623

comment

15b40b5

added a conversion between Contact from MoveIt and ContactResult from…

96b9e08

… tesseract

test

83dd551

use PlanningScene

bb46e3a

implemented planning scene for collision term too. It seems to be cor…

7417866

…rect. Just used it with FCL and singletimestep not continous collision checking yet.

ommmid force-pushed the trajopt branch from 83d5a13 to 1a28fa3 Compare May 2, 2021 10:46

Working Trajopt #1

Are you sure you want to change the base?

Working Trajopt #1

Uh oh!

Conversation

ommmid commented Jul 23, 2019 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

BryceStevenWilley left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

davetcoleman commented Jul 23, 2019

Uh oh!

davetcoleman left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

ommmid commented Jul 23, 2019

Uh oh!

ommmid commented Jul 23, 2019

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

20 participants

ommmid commented Jul 23, 2019 •

edited

Loading