Squashed improvements by AndyZe · Pull Request #58 · PickNikRobotics/trackjoint

AndyZe · 2020-06-30T15:45:23Z

This is a combination of several smaller, stale PR's. The big improvements are:

Fix a parantheses error in single_joint_generator.cpp::predictTimeToReach()
Fix sign of delta_v to backwardLimitCompensation()
Bug fix to downSample()
Seek the shortest duration possible for each joint. This should eliminate oscillations for the joint.
Synchronize multiple joints with splines. Again, this helps prevent oscillations.
c8b828c gets rid of some discontinuities while in the middle of compensation calculations

nbbrooks · 2020-07-13T03:51:42Z

This needs to be clangd

AndyZe · 2020-09-17T19:03:59Z

superceded by #63

zultron · 2020-09-18T20:46:32Z

This isn't actually superseded by #63; #63 is a PR into this branch. If you want to merge #63, the result would show up in this branch, ready for @nbbrooks to review.

If I'm messing up the workflow here, I apologize.

nbbrooks · 2020-07-12T18:58:23Z

src/single_joint_generator.cpp

      // Try increasing the duration, based on fraction of states that weren't reached successfully
      desired_duration_ =
-          1. + 0.5 * (1. - index_last_successful_ / (waypoints_.positions.size() - 1)) * desired_duration_;
+          (1. + 0.2 * (1. - index_last_successful_ / (waypoints_.positions.size() - 1))) * desired_duration_;


Good catch.

Why 0.2?

It's subjective but it should be between 0-1. A smaller fraction is going to find a solution closer to time-optimal because it searches fewer new waypoints at a time, but it will increase runtime to find the optimal number. I think adding 20% of the difference is reasonable.

Now that I think about it, I'll add a comment about that

nbbrooks · 2020-07-12T19:02:39Z

src/single_joint_generator.cpp

+  {
+    std::cout << "Limit compensation failed at the first waypoint. " <<
+              "Try a larger position_tolerance parameter or smaller timestep." << std::endl;
+  }


If index_last_successful_ == 1 would we have already returned from here with an error code in line 528?

Maybe FAILURE_TO_GENERATE_SINGLE_WAYPOINT?

good point, I will reorder those so we get the helpful error message before returning. And return FAILURE_TO_GENERATE_SINGLE_WAYPOINT, too

Actually, I played around with this and failure to compensate at the first waypoint isn't necessarily a dealbreaker. We can extend the duration and try again. I think this can be deleted completely.

nbbrooks · 2020-07-12T19:03:20Z

src/three_dof_examples.cpp

-  constexpr double waypoint_position_tolerance = 1e-5;
-  const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/trackjoint_data/output_joint";
+  constexpr double waypoint_position_tolerance = 1e-4;
+  const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint";


What if directory trackjoint_data does not exist?

I'll print a warning and return early from the function that saves results

nbbrooks · 2020-09-21T06:07:28Z

include/trackjoint/single_joint_generator.h

  /** \brief Start looking back through a velocity vector to calculate for an
   * excess velocity at limited_index. */
-  bool backwardLimitCompensation(size_t limited_index, double* excess_velocity);
+  bool backwardLimitCompensation(size_t limited_index, double excess_velocity);


This still needs to be a ptr otherwise any leftover excess_velocity computed in the function gets discarded

I don't think so, check this out...

successful_compensation = backwardLimitCompensation(index, -delta_v); if (!successful_compensation) { position_error = position_error + delta_v * configuration_.timestep;

So we keep the full delta_v if !successful_compensation. If compensation is successful, it's assumed that delta_v is zero. The logic seems good either way, to me.

Third joint acceleration definitely needs some work

…s to be exceeded

@AndyZe

* Clang tidy fixes (#53) * Clean up inline functions (#54) - Remove unnecessary inline tags - Move remaining inline function implementations into header * Update executable names in Readme (#56) * Run `clang-format` Command copied from `moveit_ci`: find . -name '*.h' -or -name '*.hpp' -or -name '*.cpp' | \ xargs /usr/bin/clang-format -i -style=file * Adjust clang-format rules and re-run Fix hideous string breaks pointed out by @AndyZe * single_joint_generator: Silence compiler warning [...]/src/single_joint_generator.cpp: In member function ‘void trackjoint::SingleJointGenerator::extendTrajectoryDuration()’: [...]/src/single_joint_generator.cpp:123:30: warning: comparison between signed and unsigned integer expressions [-Wsign-compare] for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) ~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ [...]/src/single_joint_generator.cpp:138:19: warning: unused variable ‘error_code’ [-Wunused-variable] ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_); ^~~~~~~~~~ * single_joint_generator: Silence compiler warning [...]/src/single_joint_generator.cpp: In member function ‘void trackjoint::SingleJointGenerator::extendTrajectoryDuration()’: [...]/src/single_joint_generator.cpp:123:30: error: comparison between signed and unsigned integer expressions [-Werror=sign-compare] for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx) ~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ * test/trajectory_generation_test: Silence compiler warning [...]/test/trajectory_generation_test.cpp: In member function ‘virtual void trackjoint::TrajectoryGenerationTest_VelAccelJerkLimit_Test::TestBody()’: [...]/test/trajectory_generation_test.cpp:433:16: error: unused variable ‘duration_tolerance’ [-Werror=unused-variable] const double duration_tolerance = 5e-3; ^~~~~~~~~~~~~~~~~~ * Remove junk files * .gitignore generated `.catkin-tools` directory These files were removed in previous "Remove junk files" commit; @AndyZe suggested we keep them out permanently by `.gitignore`ing them. * Change `int` to `size_t` for vector indices in tests Remove unneeded `static_cast<int>`; prepare for upcoming changes. * trajectory_generator.cpp: Fix `clang-tidy` errors [...]/src/trajectory_generator.cpp:32:58: warning: redundant boolean literal in ternary expression result [readability-simplify-boolean-expr] bool timestep_was_upsampled = (upsample_rounds_ > 0) ? true : false; ~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ upsample_rounds_ > 0 [...]/src/trajectory_generator.cpp:61:58: warning: redundant boolean literal in ternary expression result [readability-simplify-boolean-expr] bool timestep_was_upsampled = (upsample_rounds_ > 0) ? true : false; ~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ upsample_rounds_ > 0 * single_joint_generator.cpp: Fix clang-tidy errors [...]/src/single_joint_generator.cpp:116:14: warning: local copy 'spline' of the variable 'fit' is never modified; consider avoiding the copy [performance-unnecessary-copy-initialization] Spline1D spline(fit); ~~~~~~~~ ^ const & * Slence clang-tidy warnings [...]/src/single_joint_generator.cpp:279:5: warning: Value stored to 'delta_v' is never read [clang-analyzer-deadcode.DeadStores] delta_v = 0; ^ [...]/src/single_joint_generator.cpp:279:5: note: Value stored to 'delta_v' is never read 1 warning generated. * Silence clang-tidy warnings /usr/include/clang/6.0.0/include/emmintrin.h:1593:10: warning: Dereference of null pointer [clang-analyzer-core.NullDereference] return *(__m128d*)__dp; ^ [...]/test/trajectory_generation_test.cpp:1093:33: note: Calling 'TrajectoryGenerationTest::calculatePositionAccuracy' const double position_error = calculatePositionAccuracy(goal_joint_states_, output_trajectories_); ^ [...]/test/trajectory_generation_test.cpp:146:28: note: Assuming the condition is false for (size_t joint = 0; joint < trajectory.size(); ++joint) ^ [...]/test/trajectory_generation_test.cpp:146:5: note: Loop condition is false. Execution continues on line 152 for (size_t joint = 0; joint < trajectory.size(); ++joint) ^ [...]/test/trajectory_generation_test.cpp:152:20: note: Calling 'MatrixBase::norm' double error = (final_positions - goal_positions).norm(); ^ /usr/include/eigen3/Eigen/src/Core/Dot.h:107:23: note: Calling 'MatrixBase::squaredNorm' return numext::sqrt(squaredNorm()); ^ /usr/include/eigen3/Eigen/src/Core/Dot.h:95:23: note: Calling 'DenseBase::sum' return numext::real((*this).cwiseAbs2().sum()); ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:451:6: note: Left side of '||' is false if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:451:31: note: Left side of '&&' is true if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:451:3: note: Taking false branch if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0)) ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:453:10: note: Calling 'DenseBase::redux' return derived().redux(Eigen::internal::scalar_sum_op<Scalar,Scalar>()); ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:418:10: note: Calling 'redux_impl::run' return internal::redux_impl<Func, ThisEvaluator>::run(thisEval, func); ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:231:8: note: Assuming 'alignedSize' is not equal to 0 if(alignedSize) ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:231:5: note: Taking true branch if(alignedSize) ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:233:34: note: Calling 'redux_evaluator::packet' PacketScalar packet_res0 = mat.template packet<alignment,PacketScalar>(alignedStart); ^ /usr/include/eigen3/Eigen/src/Core/Redux.h:377:12: note: Calling 'unary_evaluator::packet' { return m_evaluator.template packet<LoadMode,PacketType>(index); } ^ /usr/include/eigen3/Eigen/src/Core/CoreEvaluators.h:563:31: note: Calling 'binary_evaluator::packet' return m_functor.packetOp(m_argImpl.template packet<LoadMode, PacketType>(index)); ^ /usr/include/eigen3/Eigen/src/Core/CoreEvaluators.h:734:31: note: Calling 'evaluator::packet' return m_functor.packetOp(m_lhsImpl.template packet<LoadMode,PacketType>(index), ^ /usr/include/eigen3/Eigen/src/Core/CoreEvaluators.h:204:41: note: Passing null pointer value via 1st parameter 'from' return ploadt<PacketType, LoadMode>(m_data + index); ^ /usr/include/eigen3/Eigen/src/Core/CoreEvaluators.h:204:12: note: Calling 'ploadt' return ploadt<PacketType, LoadMode>(m_data + index); ^ /usr/include/eigen3/Eigen/src/Core/GenericPacketMath.h:462:3: note: Taking true branch if(Alignment >= unpacket_traits<Packet>::alignment) ^ /usr/include/eigen3/Eigen/src/Core/GenericPacketMath.h:463:26: note: Passing null pointer value via 1st parameter 'from' return pload<Packet>(from); ^ /usr/include/eigen3/Eigen/src/Core/GenericPacketMath.h:463:12: note: Calling 'pload' return pload<Packet>(from); ^ /usr/include/eigen3/Eigen/src/Core/arch/SSE/PacketMath.h:307:124: note: Passing null pointer value via 1st parameter '__dp' template<> EIGEN_STRONG_INLINE Packet2d pload<Packet2d>(const double* from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_pd(from); } ^ /usr/include/eigen3/Eigen/src/Core/arch/SSE/PacketMath.h:307:112: note: Calling '_mm_load_pd' template<> EIGEN_STRONG_INLINE Packet2d pload<Packet2d>(const double* from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_pd(from); } ^ /usr/include/clang/6.0.0/include/emmintrin.h:1593:10: note: Dereference of null pointer return *(__m128d*)__dp; ^ Co-authored-by: Nathan Brooks <nathanbrooks@picknik.ai> Co-authored-by: AndyZe <zelenak@picknik.ai>

)

* Member variable refactor: Remove calculations from constructor SingleJointGenerator::reset does these same computations, refactor code to use this instead * SingleJointGenerator: Move configuration variables to struct Configuration variables are moved to a struct, written to by `reset()` and used by other methods. * NoisyStreamingCommand test: Record traveled trajectory Record traveled trajectory for inspection by `checkBounds()` in `TearDown()` function * NoisyStreamingCommand test: Add velocity/acceleration to next state To simulate velocity-, acceleration- and jerk-limited motion, the next state's velocity and acceleration must be updated along with position.

Duration extension can fix it

AndyZe · 2022-01-31T21:21:50Z

Outdated, closing

AndyZe requested a review from nbbrooks June 30, 2020 15:45

This was referenced Jun 30, 2020

A new function, calculateDerivativesFromVelocity() #48

Closed

Debug TimestepDidNotMatch test #49

Closed

Fix endpoint discontinuity in LimitCompensation test #50

Closed

Simple fix to backwardLimitComp if/else #57

Closed

AndyZe changed the title ~~Andyz/squashed improvements~~ Squashed improvements Jun 30, 2020

AndyZe changed the title ~~Squashed improvements~~ WIP: Squashed improvements Jul 5, 2020

AndyZe changed the title ~~WIP: Squashed improvements~~ Squashed improvements Jul 5, 2020

AndyZe force-pushed the andyz/squashed_improvements branch from fcc5a91 to a516d47 Compare July 6, 2020 13:13

AndyZe changed the title ~~Squashed improvements~~ WIP: Squashed improvements Jul 10, 2020

AndyZe changed the title ~~WIP: Squashed improvements~~ Squashed improvements Jul 10, 2020

AndyZe changed the title ~~Squashed improvements~~ WIP: Squashed improvements Aug 22, 2020

AndyZe changed the title ~~WIP: Squashed improvements~~ Squashed improvements Aug 23, 2020

AndyZe closed this Sep 17, 2020

zultron reopened this Sep 18, 2020

zultron mentioned this pull request Sep 18, 2020

Zultron/tracksuite refactor pt1 #62

Merged

nbbrooks reviewed Sep 21, 2020

View reviewed changes

AndyZe added 10 commits October 22, 2020 15:21

A new funciton, calculateDerivativesFromVelocity()

42f87ed

Fix sign of delta_v to backwardLimitComp()

911102f

Fix the streaming test/example

f50224d

Fix the skipping of some elements in downSample()

0cfe557

Add a new single-joint oscillation test

c974284

It works! Probably could be implemented more efficiently

1e7c9f0

Delete commented block of code from previous implementation

a6e8b58

Set up if-condition to skip splines, if necessary

93a7566

Fix the crash in OscillatingUR5Test

bd03348

All tests pass except DurationExtension

b9ee3dd

AndyZe and others added 25 commits October 22, 2020 15:21

Revert changes to three_dof_examples.cpp

d709306

Small fixup to streaming_example.cpp for readability

68d623c

Reset desired_duration_ in reset() function, too.

b63e02c

Delete extra loop in clipVector()

c8d4928

Change three_dof_examples to something that needs debugging

a7c0c57

Third joint acceleration definitely needs some work

Fix spline interpolation

440ee3f

Convert simple_example to a good failure case

ca03c4e

Derivative calculations in the middle of LimitComp often caused limit…

400656c

…s to be exceeded

Better estimates for desired durations

921f627

Clang format

15f1dc2

Increase position_tolerance to help pass the first waypoint

afa724e

Add checks on Timestep to every relevant unit test

f44e3bc

Add arg for whether upsampling occurred

4939981

Slightly tweak test conditions for tests that seem to be working well

f880a2f

Fix the allocation of waypoints.elapsed_times, only 2 tests fail now

e967c3b

Remove a few commented lines

26a6cc9

Fix a typo in LimitCompensation test conditions

e01a912

Initialize index_last_successful_

19791d1

Tweak simple_example.cpp for better accuracy

9929cc2

single_joint_generator: Remove trailing _ from function parameter (#65

43724a1

)

Don't need a warning for failure to comp at first waypoint

d3cf022

Duration extension can fix it

Print warning if results directory doesn't exist

060dab4

Fix small rebase mistake

479e734

AndyZe force-pushed the andyz/squashed_improvements branch from 848a54f to 479e734 Compare October 22, 2020 20:25

AndyZe added 2 commits October 22, 2020 15:40

Clang format

a28a7d4

Add comment about duration extension constant

4b4f3f3

AndyZe closed this Jan 31, 2022

Conversation

AndyZe commented Jun 30, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

nbbrooks commented Jul 13, 2020

Uh oh!

AndyZe commented Sep 17, 2020

Uh oh!

zultron commented Sep 18, 2020

Uh oh!

nbbrooks Jul 12, 2020

Choose a reason for hiding this comment

Uh oh!

AndyZe Oct 22, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

AndyZe Oct 22, 2020

Choose a reason for hiding this comment

Uh oh!

nbbrooks Jul 12, 2020

Choose a reason for hiding this comment

Uh oh!

AndyZe Oct 22, 2020

Choose a reason for hiding this comment

Uh oh!

AndyZe Oct 22, 2020

Choose a reason for hiding this comment

Uh oh!

nbbrooks Jul 12, 2020

Choose a reason for hiding this comment

Uh oh!

AndyZe Oct 22, 2020

Choose a reason for hiding this comment

Uh oh!

nbbrooks Sep 21, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

AndyZe Oct 22, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

AndyZe commented Jan 31, 2022

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

AndyZe commented Jun 30, 2020 •

edited

Loading

AndyZe Oct 22, 2020 •

edited

Loading

nbbrooks Sep 21, 2020 •

edited

Loading

AndyZe Oct 22, 2020 •

edited

Loading