From ae434790836e40598ec4ac0cd5c86171daed2928 Mon Sep 17 00:00:00 2001 From: Aditya Mandalika Date: Thu, 5 Sep 2019 17:01:15 -0700 Subject: [PATCH 1/4] change to kunz --- src/robot/ConcreteRobot.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index 7a8b4ce28b..004028e3c9 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -333,6 +333,13 @@ ConcreteRobot::getTrajectoryPostProcessor( Eigen::VectorXd velocityLimits = getVelocityLimits(*metaSkeleton); Eigen::VectorXd accelerationLimits = getAccelerationLimits(*metaSkeleton); + return std::make_shared( + velocityLimits, + accelerationLimits, + 0.1, + 0.01 + ); + return std::make_shared( velocityLimits, accelerationLimits, From 70326058484a3620989f3f72c67d9e572c03ec5f Mon Sep 17 00:00:00 2001 From: PRL Demo Date: Wed, 18 Sep 2019 18:33:45 -0700 Subject: [PATCH 2/4] uncomitted changes --- .../ConfigurationToConfiguration_to_ConfigurationToTSR.cpp | 5 +++++ src/robot/ConcreteRobot.cpp | 2 ++ src/robot/util.cpp | 5 +++++ 3 files changed, 12 insertions(+) diff --git a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp index 984c6e6575..95c1772584 100644 --- a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp +++ b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp @@ -129,6 +129,7 @@ ConfigurationToConfiguration_to_ConfigurationToTSR::plan( configurationRanker->rankConfigurations(configurations); + Eigen::VectorXd posprint(7); for (std::size_t i = 0; i < configurations.size(); ++i) { // Create ConfigurationToConfiguration Problem. @@ -142,7 +143,11 @@ ConfigurationToConfiguration_to_ConfigurationToTSR::plan( auto traj = mDelegate->plan(delegateProblem, result); if (traj) + { + mMetaSkeletonStateSpace->convertStateToPositions(configurations[i], posprint); + std::cout << posprint.transpose() << std::endl; return traj; + } } return nullptr; diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index 004028e3c9..a8b3875cf5 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -331,6 +331,8 @@ ConcreteRobot::getTrajectoryPostProcessor( double feasibilityApproxTolerance) const { Eigen::VectorXd velocityLimits = getVelocityLimits(*metaSkeleton); + for (int i = 0; i < velocityLimits.size(); ++i) + velocityLimits(i) *= 0.8; Eigen::VectorXd accelerationLimits = getAccelerationLimits(*metaSkeleton); return std::make_shared( diff --git a/src/robot/util.cpp b/src/robot/util.cpp index a76e3187f9..2e82ed192e 100644 --- a/src/robot/util.cpp +++ b/src/robot/util.cpp @@ -264,8 +264,13 @@ trajectory::TrajectoryPtr planToTSR( auto traj = snapPlanner->plan(problem, &pResult); + Eigen::VectorXd posprint(7); if (traj) + { + space->convertStateToPositions(configurations[i], posprint); + std::cout << posprint.transpose() << std::endl; return traj; + } } // Start the timer From 989f3322e730ebd867e516444e4ed92ae333be48 Mon Sep 17 00:00:00 2001 From: PRL Demo Date: Tue, 1 Oct 2019 16:56:43 -0700 Subject: [PATCH 3/4] Revert "uncomitted changes" This reverts commit 70326058484a3620989f3f72c67d9e572c03ec5f. --- .../ConfigurationToConfiguration_to_ConfigurationToTSR.cpp | 5 ----- src/robot/ConcreteRobot.cpp | 2 -- src/robot/util.cpp | 5 ----- 3 files changed, 12 deletions(-) diff --git a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp index 95c1772584..984c6e6575 100644 --- a/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp +++ b/src/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp @@ -129,7 +129,6 @@ ConfigurationToConfiguration_to_ConfigurationToTSR::plan( configurationRanker->rankConfigurations(configurations); - Eigen::VectorXd posprint(7); for (std::size_t i = 0; i < configurations.size(); ++i) { // Create ConfigurationToConfiguration Problem. @@ -143,11 +142,7 @@ ConfigurationToConfiguration_to_ConfigurationToTSR::plan( auto traj = mDelegate->plan(delegateProblem, result); if (traj) - { - mMetaSkeletonStateSpace->convertStateToPositions(configurations[i], posprint); - std::cout << posprint.transpose() << std::endl; return traj; - } } return nullptr; diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index a8b3875cf5..004028e3c9 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -331,8 +331,6 @@ ConcreteRobot::getTrajectoryPostProcessor( double feasibilityApproxTolerance) const { Eigen::VectorXd velocityLimits = getVelocityLimits(*metaSkeleton); - for (int i = 0; i < velocityLimits.size(); ++i) - velocityLimits(i) *= 0.8; Eigen::VectorXd accelerationLimits = getAccelerationLimits(*metaSkeleton); return std::make_shared( diff --git a/src/robot/util.cpp b/src/robot/util.cpp index 2e82ed192e..a76e3187f9 100644 --- a/src/robot/util.cpp +++ b/src/robot/util.cpp @@ -264,13 +264,8 @@ trajectory::TrajectoryPtr planToTSR( auto traj = snapPlanner->plan(problem, &pResult); - Eigen::VectorXd posprint(7); if (traj) - { - space->convertStateToPositions(configurations[i], posprint); - std::cout << posprint.transpose() << std::endl; return traj; - } } // Start the timer From 4a417858d78edce18c59a9cd8014724f9ae5b465 Mon Sep 17 00:00:00 2001 From: PRL Demo Date: Tue, 1 Oct 2019 16:57:22 -0700 Subject: [PATCH 4/4] Reformat. --- src/robot/ConcreteRobot.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index 004028e3c9..853c55414b 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -334,11 +334,10 @@ ConcreteRobot::getTrajectoryPostProcessor( Eigen::VectorXd accelerationLimits = getAccelerationLimits(*metaSkeleton); return std::make_shared( - velocityLimits, - accelerationLimits, - 0.1, - 0.01 - ); + velocityLimits, + accelerationLimits, + 0.1, + 0.01); return std::make_shared( velocityLimits,