Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Update stomp plugins ik code #46

Merged
merged 10 commits into from
Dec 17, 2018
Merged

Update stomp plugins ik code #46

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
77c4157
added missing install commands for some plugin libraries
jrgnicho Apr 28, 2017
7c5b00f
stomp planner now uses trac ik for all ik related tasks
jrgnicho Jun 28, 2017
e1f826c
configures the ToolGoalPose Cost function plugin from the MoveItPlann…
jrgnicho Jun 28, 2017
4f4a6d3
stomp_plugins plugins now use track ik wrapper
jrgnicho Jul 3, 2017
eb46d45
stomp optimization does not revert to lower cost previous trajectory …
jrgnicho Jul 17, 2017
03163a8
ToolGoalPose uses euler angles to make sure requested tolerances are …
jrgnicho Jul 17, 2017
f89bd41
IKSolver maps robot base coordinate system onto requested frame id
jrgnicho Jul 31, 2017
5eadb5d
minor improvements to interrupt stomp during noise filtering step
jrgnicho Aug 1, 2017
da81cf9
fixed issues with invalid error messages that resulted from processin…
jrgnicho Sep 14, 2018
0c58934
added more robust checks for cartesian goal
jrgnicho Dec 14, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 2 additions & 0 deletions stomp_core/include/stomp_core/stomp.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -182,9 +182,11 @@ class Stomp

// optimized parameters
bool parameters_valid_; /**< @brief whether or not the optimized parameters are valid */
bool parameters_valid_prev_; /**< @brief whether or not the optimized parameters from the previous iteration are valid */
double parameters_total_cost_; /**< @brief Total cost of the optimized parameters */
double current_lowest_cost_; /**< @brief Hold the lowest cost of the optimized parameters */
Eigen::MatrixXd parameters_optimized_; /**< @brief A matrix [dimensions][timesteps] of the optimized parameters. */

Eigen::MatrixXd parameters_updates_; /**< @brief A matrix [dimensions][timesteps] of the parameter updates*/
Eigen::VectorXd parameters_state_costs_; /**< @brief A vector [timesteps] of the parameters state costs */
Eigen::MatrixXd parameters_control_costs_; /**< @brief A matrix [dimensions][timesteps] of the parameters control costs*/
Expand Down
22 changes: 20 additions & 2 deletions stomp_core/src/stomp.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -248,6 +248,7 @@ bool Stomp::solve(const Eigen::MatrixXd& initial_parameters,
return false;
}

parameters_valid_prev_ = parameters_valid_;
while(current_iteration_ <= config_.num_iterations && runSingleIteration())
{

Expand Down Expand Up @@ -511,6 +512,11 @@ bool Stomp::generateNoisyRollouts()
// generate new noisy rollouts
for(auto r = 0u; r < rollouts_generate; r++)
{
if(!proceed_)
{
return false;
}

if(!task_->generateNoisyParameters(parameters_optimized_,
0,config_.num_timesteps,
current_iteration_,r,
Expand All @@ -535,6 +541,11 @@ bool Stomp::filterNoisyRollouts()
bool filtered = false;
for(auto r = 0u ; r < config_.num_rollouts; r++)
{
if(!proceed_)
{
return false;
}

if(!task_->filterNoisyParameters(0,config_.num_timesteps,current_iteration_,r,noisy_rollouts_[r].parameters_noise,filtered))
{
ROS_ERROR_STREAM("Failed to filter noisy parameters");
Expand Down Expand Up @@ -788,14 +799,21 @@ bool Stomp::computeOptimizedCost()
return false;
}

// stop optimizing when valid solution is found
if(current_lowest_cost_ > parameters_total_cost_)
{
current_lowest_cost_ = parameters_total_cost_;
parameters_valid_prev_ = parameters_valid_;
}
else
{
// reverting updates as no improvement was made
parameters_optimized_ -= parameters_updates_;
if(parameters_valid_prev_)
{
// reverting updates as no improvement was made
parameters_optimized_ -= parameters_updates_;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we need to set back parameters_valid_ here as well as the previous solution might have been valid:

      // is now valid again
      if (parameters_valid_prev_)
        parameters_valid_ = true;

or more simply

  // set back validity as well, previous might have been valid
 parameters_valid_ = parameters_valid_prev_;

this fixes "Case 1:"

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've rearrange this part so as to set the current trajectory to valid when it was reverted to the previous valid one.

parameters_valid_ = parameters_valid_prev_;

}
}

return true;
Expand Down
24 changes: 13 additions & 11 deletions stomp_moveit/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,27 +1,28 @@
cmake_minimum_required(VERSION 2.8.3)
project(stomp_moveit)

add_definitions("-std=c++11")

find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system)

find_package(catkin REQUIRED COMPONENTS
roscpp
moveit_core
moveit_ros_planning
stomp_core
cmake_modules
pluginlib
kdl_parser
trac_ik_lib
)
find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system)

add_definitions("-std=c++11")


###################################
## catkin specific configuration ##
###################################
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS moveit_ros_planning moveit_core stomp_core cmake_modules pluginlib roscpp
CATKIN_DEPENDS moveit_ros_planning moveit_core stomp_core cmake_modules pluginlib roscpp kdl_parser trac_ik_lib
DEPENDS EIGEN3
)

Expand All @@ -31,15 +32,15 @@ catkin_package(

## Specify additional locations of header files
include_directories(include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
add_library(${PROJECT_NAME}
src/stomp_optimization_task.cpp
src/stomp_planner.cpp
src/utils/polynomial.cpp
src/utils/kinematics.cpp
)

target_link_libraries(${PROJECT_NAME}
Expand All @@ -60,7 +61,7 @@ target_link_libraries(${PROJECT_NAME}_cost_functions ${catkin_LIBRARIES})

# filter plugin(s)
add_library(${PROJECT_NAME}_noisy_filters
src/noisy_filters/joint_limits.cpp
src/noisy_filters/joint_limits.cpp
src/noisy_filters/multi_trajectory_visualization.cpp
)

Expand Down Expand Up @@ -91,7 +92,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
)

## Mark executables and/or libraries for installation
install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_planner_manager ${PROJECT_NAME}_cost_functions
install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_planner_manager ${PROJECT_NAME}_cost_functions
${PROJECT_NAME}_noisy_filters ${PROJECT_NAME}_update_filters ${PROJECT_NAME}_noise_generators
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
Expand All @@ -112,3 +113,4 @@ install(
#############
## Testing ##
#############

1 change: 1 addition & 0 deletions stomp_moveit/include/stomp_moveit/noisy_filters/joint_limits.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,7 @@ class JointLimits : public StompNoisyFilter
// options
bool lock_start_;
bool lock_goal_;
bool has_goal_constraints_; /** @brief True if a joint constraint for the goal was provided*/

// start and goal
moveit::core::RobotStatePtr start_state_;
Expand Down
4 changes: 3 additions & 1 deletion stomp_moveit/include/stomp_moveit/stomp_planner.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
#include <moveit/planning_interface/planning_interface.h>
#include <stomp_core/stomp.h>
#include <stomp_moveit/stomp_optimization_task.h>
#include <stomp_moveit/utils/kinematics.h>
#include <boost/thread.hpp>
#include <ros/ros.h>

Expand Down Expand Up @@ -163,8 +164,9 @@ class StompPlanner: public planning_interface::PlanningContext
XmlRpc::XmlRpcValue config_;
stomp_core::StompConfiguration stomp_config_;

// robot environment
// robot model
moveit::core::RobotModelConstPtr robot_model_;
utils::kinematics::IKSolverPtr ik_solver_;

// ros tasks
ros::NodeHandlePtr ph_;
Expand Down