From 1b0e46b634c6b22baaf4aece998bf8e788c5be78 Mon Sep 17 00:00:00 2001 From: Reid Christopher Date: Mon, 11 Jun 2018 13:39:39 -0500 Subject: [PATCH 1/4] Added conical and confined axis constraints and tests for them --- trajopt/CMakeLists.txt | 5 + .../include/trajopt/file_write_callback.hpp | 26 ++ trajopt/include/trajopt/json_marshal.hpp | 1 + trajopt/include/trajopt/kinematic_terms.hpp | 94 +++++ .../include/trajopt/problem_description.hpp | 68 ++++ trajopt/scripts/traj_graph.py | 162 ++++++++ trajopt/src/file_write_callback.cpp | 143 +++++++ trajopt/src/json_marshal.cpp | 4 + trajopt/src/kinematic_terms.cpp | 89 +++-- trajopt/src/problem_description.cpp | 191 +++++++++ trajopt/test/angular_constraint_unit.cpp | 375 ++++++++++++++++++ trajopt/test/angular_constraint_unit.launch | 36 ++ .../basic_cartesian_plan_confined_axis.json | 114 ++++++ .../basic_cartesian_plan_conical_axis.json | 114 ++++++ trajopt/test/cast_cost_attached_unit.cpp | 1 - trajopt_examples/CMakeLists.txt | 6 + trajopt_examples/src/basic_cartesian_plan.cpp | 5 +- 17 files changed, 1399 insertions(+), 35 deletions(-) create mode 100644 trajopt/include/trajopt/file_write_callback.hpp create mode 100644 trajopt/scripts/traj_graph.py create mode 100644 trajopt/src/file_write_callback.cpp create mode 100644 trajopt/test/angular_constraint_unit.cpp create mode 100644 trajopt/test/angular_constraint_unit.launch create mode 100644 trajopt/test/basic_cartesian_plan_confined_axis.json create mode 100644 trajopt/test/basic_cartesian_plan_conical_axis.json diff --git a/trajopt/CMakeLists.txt b/trajopt/CMakeLists.txt index c14b8fed..1b4ef271 100644 --- a/trajopt/CMakeLists.txt +++ b/trajopt/CMakeLists.txt @@ -29,6 +29,7 @@ set(TRAJOPT_SOURCE_FILES src/problem_description.cpp src/utils.cpp src/plot_callback.cpp + src/file_write_callback.cpp ) catkin_package( @@ -83,4 +84,8 @@ if (CATKIN_ENABLE_TESTING) add_rostest_gtest(${PROJECT_NAME}_cast_cost_octomap_unit test/cast_cost_octomap_unit.launch test/cast_cost_octomap_unit.cpp) target_link_libraries(${PROJECT_NAME}_cast_cost_octomap_unit ${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${PCL_LIBRARIES} ${catkin_LIBRARIES}) + + add_rostest_gtest(${PROJECT_NAME}_angular_constraint_unit test/angular_constraint_unit.launch test/angular_constraint_unit.cpp) + target_link_libraries(${PROJECT_NAME}_angular_constraint_unit ${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${catkin_LIBRARIES}) + endif() diff --git a/trajopt/include/trajopt/file_write_callback.hpp b/trajopt/include/trajopt/file_write_callback.hpp new file mode 100644 index 00000000..cabbde65 --- /dev/null +++ b/trajopt/include/trajopt/file_write_callback.hpp @@ -0,0 +1,26 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +typedef std::shared_ptr ofstreamPtr; + +using namespace Eigen; +using namespace util; +using namespace std; +namespace trajopt +{ + + Optimizer::Callback WriteCallback(string file_name,TrajOptProbPtr prob, vector joint_names, + vector pose_inverses, Affine3d tcp = Affine3d::Identity(), bool angle_axis = true, bool degrees = true); + +} diff --git a/trajopt/include/trajopt/json_marshal.hpp b/trajopt/include/trajopt/json_marshal.hpp index c74915f6..4eb8510b 100644 --- a/trajopt/include/trajopt/json_marshal.hpp +++ b/trajopt/include/trajopt/json_marshal.hpp @@ -12,6 +12,7 @@ namespace json_marshal TRAJOPT_API void fromJson(const Json::Value& v, bool& ref); TRAJOPT_API void fromJson(const Json::Value& v, int& ref); TRAJOPT_API void fromJson(const Json::Value& v, double& ref); +TRAJOPT_API void fromJson(const Json::Value& v, char& ref); TRAJOPT_API void fromJson(const Json::Value& v, std::string& ref); TRAJOPT_API void fromJson(const Json::Value& v, Eigen::Vector3d& ref); TRAJOPT_API void fromJson(const Json::Value& v, Eigen::Vector4d& ref); diff --git a/trajopt/include/trajopt/kinematic_terms.hpp b/trajopt/include/trajopt/kinematic_terms.hpp index 4b0be7a5..8c7abd25 100644 --- a/trajopt/include/trajopt/kinematic_terms.hpp +++ b/trajopt/include/trajopt/kinematic_terms.hpp @@ -104,4 +104,98 @@ struct CartVelCalculator : VectorOfVector VectorXd operator()(const VectorXd& dof_vals) const; }; + +/** + * @brief The ConfinedAxisErrCalculator is a truct whose operator() calculates error of a given + * pose with respect ot the confined rotation along the defined axis. + */ +struct ConfinedAxisErrCalculator : public VectorOfVector { + + Eigen::Affine3d pose_inv_; /**< param The inverse of the desired pose */ + tesseract::BasicKinConstPtr manip_; /**< Kinematics object */ + tesseract::BasicEnvConstPtr env_; /**< Environment object */ + std::string link_; /**< Link of the robot referred to */ + Eigen::Affine3d tcp_; /**< Tool center point */ + + char axis_; /**< Axis of rotation being provided with a tolerance */ + double tol_; /**< Tolerance of the rotation stored specific to the error calculation + NOTE: this is not an angle, but a value used in error calculation stored to avoid conversion using the angle during each call */ + + /** + * @brief ConfinedAxisErrCalculator Constructor + * @param pose Desired pose + * @param manip + * @param env + * @param link + * @param axis Axis of rotation + * @param tol_angle Tolerance in radians + * @param tcp Tool center point + */ + ConfinedAxisErrCalculator(const Eigen::Affine3d& pose, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env, + std::string link, char axis, double tol_angle, Eigen::Affine3d tcp = Eigen::Affine3d::Identity()) : + pose_inv_(pose.inverse()), + manip_(manip), + env_(env), + link_(link), + tcp_(tcp), + axis_(axis) + { + tol_ = sin(tol_angle*M_PI/180.0 / 2.0); + } + + /** + * @brief operator () Calculates error + * @param dof_vals Values of the joints for forward kinematics + * @return 1D vector of error beyond the allowed rotation + */ + VectorXd operator()(const VectorXd& dof_vals) const; + +}; + +/** + * @brief The ConicalAxisErrCalculator is a struct whose operator() returns the error of the + * given pose with respect to the conical constraint defined + * + */ +struct ConicalAxisErrCalculator : public VectorOfVector { + Eigen::Affine3d pose_inv_; /**< Inverse of the desired pose */ + tesseract::BasicKinConstPtr manip_; /**< Kinematics object */ + tesseract::BasicEnvConstPtr env_; /**< Environment object */ + std::string link_; /**< The link of the robot reggered to */ + Eigen::Affine3d tcp_; /**< Tool center point */ + + char axis_; /**< Axis the conical tolreance is applied to */ + double tol_; /**< Internal representation of the tolerance. + NOTE: this is not an angle, but a value used in error calculation stored to avoid conversion using the angle during each call */ + + /** + * @brief ConicalAxisErrCalculator + * @param pose Desired pose + * @param manip Kinematics Object + * @param env Environment object + * @param link Link of the robot the term applies to + * @param axis Axis to define the conical tolerance around + * @param tol_angle Tolerance angle in degrees + * @param tcp Tool center point + */ + ConicalAxisErrCalculator(const Eigen::Affine3d& pose, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env, + std::string link, char axis, double tol_angle, Eigen::Affine3d tcp = Eigen::Affine3d::Identity()) : + pose_inv_(pose.inverse()), + manip_(manip), + env_(env), + link_(link), + tcp_(tcp), + axis_(axis) + { + tol_ = cos(tol_angle*M_PI/180.0); + } + + /** + * @brief operator () + * @param dof_vals + * @return + */ + VectorXd operator()(const VectorXd& dof_vals) const; +}; + } diff --git a/trajopt/include/trajopt/problem_description.hpp b/trajopt/include/trajopt/problem_description.hpp index a7182771..002bc993 100644 --- a/trajopt/include/trajopt/problem_description.hpp +++ b/trajopt/include/trajopt/problem_description.hpp @@ -12,6 +12,7 @@ struct OptResults; namespace trajopt { + using namespace json_marshal; using namespace Json; @@ -322,4 +323,71 @@ struct JointConstraintInfo : public TermInfo, public MakesConstraint void hatch(TrajOptProb& prob); DEFINE_CREATE(JointConstraintInfo) }; + +/** + * @brief The ConfinedAxisTermInfo struct contains information to create constraints or costs + * using a tolerance for the rotation about an axis + */ +struct ConfinedAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost { + int timestep; /**< The timestep of this term in the trajectory */ + Vector3d xyz; /**< Cartesian position coordinate vector for the pose */ + Vector4d wxyz; /**< 4D vector containing w, x, y, and z quaternion components for the pose (in that order) */ + Vector3d pos_coeffs; /**< Coefficients multiplied by xyz errors during evaluation of cost/constraint values */ + double confined_coeff; /**< Coefficient multiplied by error of the rotation beyond tolerance */ + double axis_coeff; /**< Coefficient multiplied by the errors of the rotation axis from the specified axis */ + + string link; /**< Link of the robot the term refers to */ + Eigen::Affine3d tcp; /**< Tool center point */ + + char axis; /**< Axis allowed to rotate */ + double tol; /**< Rotation acceptable in degrees */ + + ConfinedAxisTermInfo(); + + /** + * @brief fromJson Constructs the term from a Json file + */ + void fromJson(ProblemConstructionInfo &pci, const Value& v); + + /** + * @brief hatch Add the proper cost/constraint functions to the problem + * @param prob The optimization problems to add the term to + */ + void hatch(TrajOptProb& prob); + DEFINE_CREATE(ConfinedAxisTermInfo) +}; + +/** + * @brief The ConicalAxisTermInfo struct contains information to create constraints or costs + * using a conical tolerance about an axis + */ +struct ConicalAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost { + int timestep; /**< The timestep of this term in the trajectory */ + Vector3d xyz; /**< Cartesian position coordinate vector for the pose */ + Vector4d wxyz; /**< 4D vector containing w, x, y, and z quaternion components for the pose (in that order) */ + Vector3d pos_coeffs; /**< Coefficients multiplied by xyz errors during evaluation of cost/constraint values */ + double conical_coeff; /**< Coefficient multiplied by error of the axis orientation beyond tolerance */ + double axis_coeff; /**< Coefficient multiplied by the rotation error about the axis in the conical tolerance */ + + string link; /**< Link of the robot the term refers to */ + Eigen::Affine3d tcp; /**< Tool center point */ + + char axis; /**< Axis given a conical tolerance */ + double tol; /**< Cone angle in degrees */ + + ConicalAxisTermInfo(); + + /** + * @brief fromJson Constructs the term from a Json file + */ + void fromJson(ProblemConstructionInfo &pci, const Value& v); + + /** + * @brief hatch Add the proper cost/constraint functions to the problem + * @param prob The optimization problems to add the term to + */ + void hatch(TrajOptProb& prob); + DEFINE_CREATE(ConicalAxisTermInfo) +}; + } diff --git a/trajopt/scripts/traj_graph.py b/trajopt/scripts/traj_graph.py new file mode 100644 index 00000000..40fbf39c --- /dev/null +++ b/trajopt/scripts/traj_graph.py @@ -0,0 +1,162 @@ +import numpy as np +import matplotlib.pyplot as plt +import argparse +import pandas as pd + +# parse file names from command line +parser = argparse.ArgumentParser(description="Plot before and after trajectory") +parser.add_argument('a', type=str, help='First file') +parser.add_argument('b', type=str, help='Second file') +args = parser.parse_args() + +# read the files +data_a = pd.read_csv(args.a, skip_blank_lines=False) +data_b = pd.read_csv(args.b, skip_blank_lines=False) + +# get the dimensions of each of the files +shape_a = data_a.shape +rows_a = shape_a[0] +cols_a = shape_a[1] + +shape_b = data_b.shape +rows_b = shape_b[0] +cols_b = shape_b[1] + +# check that the files have the same number of columns +if (cols_a != cols_b): + print('The files do not have the same number of columns. Exiting'); + +# check that the files' columns hold the same information +if ((data_a.columns != data_b.columns).any()): + print('The column names for these files do not match. Exiting') + exit(-1) + +num_dofs = cols_a - 14 +cols = cols_a + +# check that the robots have the same number of poses set for the path +num_poses_a = 0 +num_poses_b = 0 + +var = data_a.iloc[0,0] +while (not np.isnan(var)): + num_poses_a += 1 + var = data_a.iloc[num_poses_a, 0] + +var = data_b.iloc[0,0] +while (not np.isnan(var)): + num_poses_b += 1 + var = data_b.iloc[num_poses_b, 0] + +if (num_poses_a != num_poses_b): + print('These two files do not describe the same path. Exiting') + exit(-1) + +# get the data for the final trajectories in each file +lower_a = rows_a - 1 - num_poses_a; +upper_a = rows_a - 1; +dof_vals_a = np.matrix(data_a.iloc[lower_a:upper_a, 0:num_dofs]) +pose_vals_a = np.matrix(data_a.iloc[lower_a:upper_a, num_dofs:(num_dofs + 7)]) +err_vals_a = np.matrix(data_a.iloc[lower_a:upper_a, (num_dofs + 7):(num_dofs + 14)]) + +lower_b = rows_b - 1 - num_poses_b; +upper_b = rows_b - 1; +dof_vals_b = np.matrix(data_b.iloc[lower_b:upper_b, 0:num_dofs]) +pose_vals_b = np.matrix(data_b.iloc[lower_b:upper_b, num_dofs:(num_dofs + 7)]) +err_vals_b = np.matrix(data_b.iloc[lower_b:upper_b, (num_dofs + 7):(num_dofs + 14)]) + +x = np.transpose(np.array(range(0,num_poses_a), ndmin=2)) + +# start at figure 1 and plot the DOFs +current_fig = 1 +plots_left = num_dofs + +# plot DOF values +for iter in range(0, (num_dofs >> 2) + 1): + + # open a new figure and determine the number of subplots + plt.figure(current_fig) + num_to_plot = min(plots_left, 4) + + # plots the subplots for the figure + for i in range(0, num_to_plot): + index = i + iter * 4 + cur_plot = plt.subplot(num_to_plot, 1, i + 1) + plt.plot(x, dof_vals_a[:, index], x, dof_vals_b[:, index]) + + dof_name = data_a.columns[i] + plt.legend([dof_name + '_' + args.a[:-4], dof_name + '_' + args.b[:-4]]) + + ylim = cur_plot.get_ylim() + diff = (ylim[1] - ylim[0]) * 0.1 + cur_plot.set_ylim([ylim[0] - diff, ylim[1] + diff]) + + # set the plot to fullscreen + #figManager = plt.get_current_fig_manager() + #figManager.full_screen_toggle() + + # increment the figure number and note the plots we finished + current_fig += 1 + plots_left -= 4 + +plots_left = 7 + +# plot pose values +for iter in range(0, 2): + + # open a new figure and determine the number of subplots + plt.figure(current_fig) + num_to_plot = min(plots_left, 4) + + # plots the subplots for the figure + for i in range(0, num_to_plot): + index = i + iter * 4 + cur_plot = plt.subplot(num_to_plot, 1, i + 1) + plt.plot(x, pose_vals_a[:, index], x, pose_vals_b[:, index]) + + pose_name = data_a.columns[index + num_dofs] + plt.legend([pose_name + '_' + args.a[:-4], pose_name + '_' + args.b[:-4]]) + + ylim = cur_plot.get_ylim() + diff = (ylim[1] - ylim[0]) * 0.1 + cur_plot.set_ylim([ylim[0] - diff, ylim[1] + diff]) + + # set the plot to fullscreen + #figManager = plt.get_current_fig_manager() + #figManager.full_screen_toggle(); + + # increment the figure number and note the plots we finished + current_fig += 1 + plots_left -= 4 + +plots_left = 7 + +# plot err values +for iter in range(0, 2): + + # open a new figure and determine the number of subplots + plt.figure(current_fig) + num_to_plot = min(plots_left, 4) + + # plots the subplots for the figure + for i in range(0, num_to_plot): + index = i + iter * 4 + cur_plot = plt.subplot(num_to_plot, 1, i + 1) + plt.plot(x, err_vals_a[:, index], x, err_vals_b[:, index]) + + err_name = data_a.columns[index + 7 + num_dofs] + plt.legend([err_name + '_' + args.a[:-4], err_name + '_' + args.b[:-4]]) + + ylim = cur_plot.get_ylim() + diff = (ylim[1] - ylim[0]) * 0.1 + cur_plot.set_ylim([ylim[0] - diff, ylim[1] + diff]) + + # set the plot to fullscreen + #figManager = plt.get_current_fig_manager() + #figManager.full_screen_toggle() + + # increment the figure number and note the plots we finished + current_fig += 1 + plots_left -= 4 + +plt.show() diff --git a/trajopt/src/file_write_callback.cpp b/trajopt/src/file_write_callback.cpp new file mode 100644 index 00000000..b3001d72 --- /dev/null +++ b/trajopt/src/file_write_callback.cpp @@ -0,0 +1,143 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Eigen; +using namespace util; +using namespace std; +namespace trajopt +{ + + void WriteFile(ofstreamPtr file, vector pose_inverses, Affine3d tcp, TrajOptProbPtr prob, DblVec& x, + bool angle_axis, bool degrees) + { + // get joint info + //get pose info + //write to file + auto env = prob->GetEnv(); + auto manip = prob->GetKin(); + auto change_base = env->getLinkTransform(manip->getBaseLinkName()); + + TrajArray traj = getTraj(x, prob->GetVars()); + for (auto i = 0; i < traj.rows(); i++) { + VectorXd joint_angles(traj.cols()); + for (auto j = 0; j < traj.cols(); j++) { + if (j != 0) { + *file << ','; + } + *file << traj(i,j); + + joint_angles(j) = traj(i,j); + } + + Affine3d pose; + manip->calcFwdKin(pose, change_base, joint_angles); + + Vector4d rot_vec; + if (angle_axis) { + AngleAxisd aa(pose.rotation()); + Vector3d axis = aa.axis(); + rot_vec(0) = degrees ? aa.angle() * 180.0/M_PI : aa.angle(); + rot_vec(1) = axis(0); + rot_vec(2) = axis(1); + rot_vec(3) = axis(2); + + } + else { + Quaterniond q(pose.rotation()); + rot_vec(0) = q.w(); + rot_vec(1) = q.x(); + rot_vec(2) = q.y(); + rot_vec(3) = q.z(); + } + + VectorXd pose_vec = concat(pose.translation(), rot_vec); + for (auto i = 0;i < pose_vec.size(); i++) { + *file << ',' << pose_vec(i); + } + + Affine3d pose_inv = pose_inverses.at(i); + Affine3d err = pose_inv * (pose * tcp); + + Vector4d rot_err; + if (angle_axis) { + AngleAxisd aa(err.rotation()); + rot_err(0) = degrees ? aa.angle() * 180.0/M_PI : aa.angle(); + + Vector3d axis = aa.axis(); + rot_err(1) = axis(0); + rot_err(2) = axis(1); + rot_err(3) = axis(2); + } + else { + Quaterniond q(err.rotation()); + rot_err(0) = q.w(); + rot_err(1) = q.x(); + rot_err(2) = q.y(); + rot_err(3) = q.z(); + } + + VectorXd err_vec = concat(err.translation(), rot_err); + for (auto j = 0; j < err_vec.size(); j++) { + *file << "," << err_vec(j); + } + + *file << endl; + } + *file << endl; + } + + Optimizer::Callback WriteCallback(string file_name, TrajOptProbPtr prob, vector joint_names, vector pose_inverses, + Affine3d tcp /*=identity*/, bool angle_axis /*=true*/, bool degrees /*=true*/) + { + + ofstreamPtr file(new ofstream(file_name, ofstream::trunc)); + ROS_ERROR(file->good() ? "File successfully opened" : "Error opening file"); + + for (size_t i = 0; i < joint_names.size(); i++) { + if (i != 0) { + *file << ','; + } + *file << joint_names.at(i); + } + + vector pose_str = angle_axis ? vector{"x", "y", "z", "angle", "axis_x", "axis_y", "axis_z"} : + vector{"x", "y", "z", "q_w", "q_x", "q_y", "q_z"}; + + vector err_str(pose_str.size()); + for (size_t i = 0; i < err_str.size(); i++) { + err_str.at(i) = pose_str.at(i) + "_err"; + } + + for (size_t i = 0; i < pose_str.size(); i++) { + *file << ',' << pose_str.at(i); + } + + for (size_t i = 0; i < err_str.size(); i++) { + *file << ',' << err_str.at(i); + } + + *file << endl; + + // return callback function + return bind(&WriteFile, + file, + pose_inverses, + tcp, + prob, + placeholders::_2, + angle_axis, + degrees); + } + +} diff --git a/trajopt/src/json_marshal.cpp b/trajopt/src/json_marshal.cpp index 67f52695..686ad552 100644 --- a/trajopt/src/json_marshal.cpp +++ b/trajopt/src/json_marshal.cpp @@ -42,6 +42,10 @@ void fromJson(const Json::Value& v, double& ref) } } +void fromJson(const Json::Value& v, char& ref) { + ref = *(v.asCString()); +} + void fromJson(const Json::Value& v, std::string& ref) { try diff --git a/trajopt/src/kinematic_terms.cpp b/trajopt/src/kinematic_terms.cpp index 2e17fe95..db5039b6 100644 --- a/trajopt/src/kinematic_terms.cpp +++ b/trajopt/src/kinematic_terms.cpp @@ -10,6 +10,8 @@ #include #include + +#include using namespace std; using namespace sco; using namespace Eigen; @@ -88,8 +90,13 @@ VectorXd StaticCartPoseErrCalculator::operator()(const VectorXd& dof_vals) const env_->getState(manip_->getJointNames(), dof_vals)->transforms.at(manip_->getBaseLinkName()))); manip_->calcFwdKin(new_pose, change_base, dof_vals, link_, *state); + // calculate the err of the current pose Affine3d pose_err = pose_inv_ * (new_pose * tcp_); + + // get the quaternion representation of the rotation error Quaterniond q(pose_err.rotation()); + + // construct a 6D vector containing orientation and positional errors VectorXd err = concat(Vector3d(q.x(), q.y(), q.z()), pose_err.translation()); return err; } @@ -171,37 +178,55 @@ VectorXd CartVelCalculator::operator()(const VectorXd& dof_vals) const return out; } -#if 0 -CartVelConstraint::CartVelConstraint(const VarVector& step0vars, const VarVector& step1vars, RobotAndDOFPtr manip, KinBody::LinkPtr link, double distlimit) : - ConstraintFromFunc(VectorOfVectorPtr(new CartVelCalculator(manip, link, distlimit)), - MatrixOfVectorPtr(new CartVelJacCalculator(manip, link, distlimit)), concat(step0vars, step1vars), VectorXd::Ones(0), INEQ, "CartVel") -{} // TODO coeffs -#endif +VectorXd ConfinedAxisErrCalculator::operator()(const VectorXd& dof_vals) const { + // calculate the current pose given the DOF values for the robot + Affine3d new_pose, change_base; + change_base = env_->getLinkTransform(manip_->getBaseLinkName()); + manip_->calcFwdKin(new_pose, change_base, dof_vals, link_, *env_->getState()); + + // get the error of the current pose + Affine3d pose_err = pose_inv_ * (new_pose * tcp_); + Quaterniond q(pose_err.rotation()); + VectorXd err(1); + + // determine the error of the rotation + switch(axis_) { + case 'x': err(0) = fabs(q.x()) - tol_; + break; + case 'y': err(0) = fabs(q.y()) - tol_; + break; + case 'z': err(0) = fabs(q.z()) - tol_; + break; + } + + return err; +} + +VectorXd ConicalAxisErrCalculator::operator()(const VectorXd& dof_vals) const { + // calculate the current pose given the DOF values for the robot + Affine3d new_pose, change_base; + change_base = env_->getLinkTransform(manip_->getBaseLinkName()); + manip_->calcFwdKin(new_pose, change_base, dof_vals, link_, *env_->getState()); + + // get the error of the current pose + Affine3d pose_err = pose_inv_ * (new_pose * tcp_); + + // get the orientation matrix of the error + Matrix3d orientation(pose_err.rotation()); + VectorXd err(1); + + // determine the error of the conical axis + // tol_ = cos(tol_angle), so when angle < tol_angle, err < 0, and vice versa + switch(axis_) { + case 'x': err(0) = tol_ - orientation(0, 0); + break; + case 'y': err(0) = tol_ - orientation(1, 1); + break; + case 'z': err(0) = tol_ - orientation(2, 2); + break; + } + + return err; +} -#if 0 -struct UpErrorCalculator { - Vector3d dir_local_; - Vector3d goal_dir_world_; - RobotAndDOFPtr manip_; - OR::KinBody::LinkPtr link_; - MatrixXd perp_basis_; // 2x3 matrix perpendicular to goal_dir_world - UpErrorCalculator(const Vector3d& dir_local, const Vector3d& goal_dir_world, RobotAndDOFPtr manip, KinBody::LinkPtr link) : - dir_local_(dir_local), - goal_dir_world_(goal_dir_world), - manip_(manip), - link_(link) - { - Vector3d perp0 = goal_dir_world_.cross(Vector3d::Random()).normalized(); - Vector3d perp1 = goal_dir_world_.cross(perp0); - perp_basis_.resize(2,3); - perp_basis_.row(0) = perp0.transpose(); - perp_basis_.row(1) = perp1.transpose(); - } - VectorXd operator()(const VectorXd& dof_vals) { - manip_->SetDOFValues(toDblVec(dof_vals)); - OR::Transform newpose = link_->GetTransform(); - return perp_basis_*(toRot(newpose.rot) * dir_local_ - goal_dir_world_); - } -}; -#endif } diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp index 93b23aaa..c67a4a2f 100644 --- a/trajopt/src/problem_description.cpp +++ b/trajopt/src/problem_description.cpp @@ -55,6 +55,8 @@ void RegisterMakers() TermInfo::RegisterMaker("joint", &JointConstraintInfo::create); TermInfo::RegisterMaker("cart_vel", &CartVelCntInfo::create); TermInfo::RegisterMaker("joint_vel_limits", &JointVelConstraintInfo::create); + TermInfo::RegisterMaker("confined_axis", &ConfinedAxisTermInfo::create); + TermInfo::RegisterMaker("conical_axis", &ConicalAxisTermInfo::create); gRegisteredMakers = true; } @@ -801,4 +803,193 @@ void JointConstraintInfo::hatch(TrajOptProb& prob) prob.addLinearConstraint(exprSub(AffExpr(vars[j]), vals[j]), EQ); } } + +// initialize with basic default values +ConfinedAxisTermInfo::ConfinedAxisTermInfo() { + pos_coeffs = Vector3d::Ones(); + axis_coeff = 1.0; + confined_coeff = 1.0; + tcp.setIdentity(); +} + +// get the term information from a json value +void ConfinedAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value& v) { + // make sure params is a member of the Json value provided + FAIL_IF_FALSE(v.isMember("params")); + + // provide default position and orienation for the tcp (tool center point) + Vector3d tcp_xyz = Vector3d::Zero(); + Vector4d tcp_wxyz = Vector4d(1, 0, 0, 0); + + // get the params value and read the parameters fmro the file into the object members + const Value& params = v["params"]; + childFromJson(params, timestep, "timestep", pci.basic_info.n_steps-1); + childFromJson(params, xyz,"xyz"); + childFromJson(params, wxyz,"wxyz"); + childFromJson(params, pos_coeffs, "pos_coeffs", (Vector3d)Vector3d::Ones()); + childFromJson(params, axis_coeff, "axis_coeff", (double)1.0); + childFromJson(params, confined_coeff, "confined_coeff", (double)1.0); + childFromJson(params, link, "link"); + childFromJson(params, axis, "axis"); + childFromJson(params, tol, "tolerance"); + + // construct the tcp Affine matrix from its components + childFromJson(params, tcp_xyz, "tcp,xyz", tcp_xyz); + childFromJson(params, tcp_wxyz, "tcp_xwyz", tcp_wxyz); + Eigen::Quaterniond q(tcp_wxyz(0), tcp_wxyz(1), tcp_wxyz(2), tcp_wxyz(3)); + tcp.linear() = q.matrix(); + tcp.translation() = tcp_xyz; + + // make sure the link name provided is valid + const std::vector& link_names = pci.kin->getLinkNames(); + if (std::find(link_names.begin(), link_names.end(),link)==link_names.end()) { + PRINT_AND_THROW(boost::format("invalid link name: %s")%link); + } + + // make sure there are no extra items in the params value + const char* all_fields[] = {"timestep", "xyz", "wxyz", "pos_coeffs", "axis_coeff", "confined_coeff", "link", + "tolerance", "axis", "tcp_xyz", "tcp_wxyz"}; + ensure_only_members(params, all_fields, sizeof(all_fields)/sizeof(char*)); +} + +// add the term to the problem description +void ConfinedAxisTermInfo::hatch(TrajOptProb& prob) { + // construct the desired pose + Eigen::Affine3d input_pose; + Eigen::Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + input_pose.linear() = q.matrix(); + input_pose.translation() = xyz; + + // create the equality and inequality error functions to be used + VectorOfVectorPtr f_eq(new StaticCartPoseErrCalculator(input_pose, prob.GetKin(), prob.GetEnv(), link, tcp)); + VectorOfVectorPtr f_ineq(new ConfinedAxisErrCalculator(input_pose, prob.GetKin(), prob.GetEnv(), link, axis, tol, tcp)); + + // construct the coefficient vectors for the errors + VectorXd pose_coeffs = concat(Vector3d(0.0, 0.0, 0.0), pos_coeffs); + VectorXd ineq_coeffs(1); + + // the inequality coefficient is the one referrring to the error outside the confined rotation + ineq_coeffs(0) = confined_coeff; + + // the error corresponding to the axis of rotation having the proper orientation corresponds to the + // values of the other 2 quaternion error components + switch(axis) { + case 'x': + pose_coeffs(1) = axis_coeff; + pose_coeffs(2) = axis_coeff; + break; + case 'y': + pose_coeffs(0) = axis_coeff; + pose_coeffs(2) = axis_coeff; + break; + case 'z': + pose_coeffs(0) = axis_coeff; + pose_coeffs(1) = axis_coeff; + break; + } + + // add the costs or constraints depending on the term type + if (term_type == TT_COST) { + prob.addCost(CostPtr(new CostFromErrFunc(f_eq, prob.GetVarRow(timestep), pose_coeffs, ABS, name))); + prob.addCost(CostPtr(new CostFromErrFunc(f_ineq, prob.GetVarRow(timestep), ineq_coeffs, HINGE, name))); + } + else if (term_type == TT_CNT) { + prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_eq, prob.GetVarRow(timestep), pose_coeffs, EQ, name))); + prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_ineq, prob.GetVarRow(timestep), ineq_coeffs, INEQ, name))); + } +} + +// initialize with basic default values +ConicalAxisTermInfo::ConicalAxisTermInfo() { + pos_coeffs = Vector3d::Ones(); + axis_coeff = 1.0; + conical_coeff = 1.0; + tcp.setIdentity(); +} + +// construct the term from a Json file +void ConicalAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value &v) { + // make sure params is a member of the Json value provided + FAIL_IF_FALSE(v.isMember("params")); + + // provide default position and orienation for the tcp (tool center point) + Vector3d tcp_xyz = Vector3d::Zero(); + Vector4d tcp_wxyz = Vector4d(1, 0, 0, 0); + + // get the params value and read the parameters fmro the file into the object members + const Value& params = v["params"]; + childFromJson(params, timestep, "timestep", pci.basic_info.n_steps-1); + childFromJson(params, xyz,"xyz"); + childFromJson(params, wxyz,"wxyz"); + childFromJson(params, pos_coeffs, "pos_coeffs", (Vector3d)Vector3d::Ones()); + childFromJson(params, axis_coeff, "axis_coeff", (double)1.0); + childFromJson(params, conical_coeff, "conical_coeff", (double)1.0); + childFromJson(params, link, "link"); + childFromJson(params, axis, "axis"); + childFromJson(params, tol, "tolerance"); + + // construct the tcp Affine matrix from its components + childFromJson(params, tcp_xyz, "tcp,xyz", tcp_xyz); + childFromJson(params, tcp_wxyz, "tcp_xwyz", tcp_wxyz); + Eigen::Quaterniond q(tcp_wxyz(0), tcp_wxyz(1), tcp_wxyz(2), tcp_wxyz(3)); + tcp.linear() = q.matrix(); + tcp.translation() = tcp_xyz; + + // make sure the link name provided is valid + const std::vector& link_names = pci.kin->getLinkNames(); + if (std::find(link_names.begin(), link_names.end(),link)==link_names.end()) { + PRINT_AND_THROW(boost::format("invalid link name: %s")%link); + } + + // make sure there are no extra items in the params value + const char* all_fields[] = {"timestep", "xyz", "wxyz", "pos_coeffs", "axis_coeff", "conical_coeff", "link", + "tolerance", "axis", "tcp_xyz", "tcp_wxyz"}; + ensure_only_members(params, all_fields, sizeof(all_fields)/sizeof(char*)); +} + +// add the term to the problem description +void ConicalAxisTermInfo::hatch(TrajOptProb &prob) { + // construct the desired pose + Eigen::Affine3d input_pose; + Eigen::Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + input_pose.linear() = q.matrix(); + input_pose.translation() = xyz; + + // create the equality and inequality error functions to be used + VectorOfVectorPtr f_eq(new StaticCartPoseErrCalculator(input_pose, prob.GetKin(), prob.GetEnv(), link, tcp)); + VectorOfVectorPtr f_ineq(new ConicalAxisErrCalculator(input_pose, prob.GetKin(), prob.GetEnv(), link, axis, tol, tcp)); + + // construct the coefficient vectors for the errors + VectorXd pose_coeffs = concat(Vector3d(0.0, 0.0, 0.0), pos_coeffs); + VectorXd ineq_coeffs(1); + + // the inequality coefficient is the one referrring to the error outside the conical tolerance + ineq_coeffs(0) = conical_coeff; + + // the error corresponding to the rotation about the axis in the cone is given by + // the quaternion component of that axis + switch(axis) { + case 'x': + pose_coeffs(0) = axis_coeff; + break; + case 'y': + pose_coeffs(1) = axis_coeff; + break; + case 'z': + pose_coeffs(2) = axis_coeff; + break; + } + + // add the costs or constraints depending on the term type + if (term_type == TT_COST) { + prob.addCost(CostPtr(new CostFromErrFunc(f_eq, prob.GetVarRow(timestep), pose_coeffs, ABS, name))); + prob.addCost(CostPtr(new CostFromErrFunc(f_ineq, prob.GetVarRow(timestep), ineq_coeffs, HINGE, name))); + } + else if (term_type == TT_CNT) { + prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_eq, prob.GetVarRow(timestep), pose_coeffs, EQ, name))); + prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_ineq, prob.GetVarRow(timestep), ineq_coeffs, INEQ, name))); + } + +} + } diff --git a/trajopt/test/angular_constraint_unit.cpp b/trajopt/test/angular_constraint_unit.cpp new file mode 100644 index 00000000..426ce934 --- /dev/null +++ b/trajopt/test/angular_constraint_unit.cpp @@ -0,0 +1,375 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace trajopt; +using namespace tesseract; + +const std::string ROBOT_DESCRIPTION_PARAM = "robot_description"; /**< Default ROS parameter for robot description */ +const std::string ROBOT_SEMANTIC_PARAM = "robot_description_semantic"; /**< Default ROS parameter for robot description */ + +bool plotting_ = false; +int steps_ = 5; + +struct testInfo { + std::string method; + std::string constraint; + + testInfo(std::string m, std::string c) : method(m), constraint(c) {} +}; + +class AngularConstraintTest : public testing::TestWithParam { +public: + ros::NodeHandle nh_; + urdf::ModelInterfaceSharedPtr urdf_model_; /**< URDF Model */ + srdf::ModelSharedPtr srdf_model_; /**< SRDF Model */ + tesseract_ros::KDLEnvPtr env_; /**< Trajopt Basic Environment */ + tesseract_ros::ROSBasicPlottingPtr plotter_; /**< Trajopt Plotter */ + std::vector pose_inverses_; + std::string constraint_type_; + double tol_; + + AngularConstraintTest() : pose_inverses_(steps_) {} + + TrajOptProbPtr jsonMethod() + { + std::string config_file; + nh_.getParam(constraint_type_ + "_json_file", config_file); + + Json::Value root; + Json::Reader reader; + bool parse_success = reader.parse(config_file.c_str(), root); + if (!parse_success) + { + ROS_FATAL("Failed to load trajopt json file from ros parameter"); + } + + ProblemConstructionInfo pci(env_); + pci.fromJson(root); + for (int i = 0; i < steps_; i++) { + Affine3d cur_pose; + if (constraint_type_ == "confined") { + std::shared_ptr pose = static_pointer_cast(pci.cnt_infos.at(i)); + cur_pose.translation() = pose->xyz; + Quaterniond q(pose->wxyz(0), pose->wxyz(1), pose->wxyz(2), pose->wxyz(3)); + cur_pose.linear() = q.matrix(); + pose_inverses_.insert(pose_inverses_.begin() + i, cur_pose.inverse()); + } + else if (constraint_type_ == "conical") { + std::shared_ptr pose = static_pointer_cast(pci.cnt_infos.at(i)); + cur_pose.translation() = pose->xyz; + Quaterniond q(pose->wxyz(0), pose->wxyz(1), pose->wxyz(2), pose->wxyz(3)); + cur_pose.linear() = q.matrix(); + pose_inverses_.insert(pose_inverses_.begin() + i, cur_pose.inverse()); + } + else { + ROS_ERROR("%s is not a valid constraint type. Exiting", constraint_type_.c_str()); + exit(-1); + } + } + + return ConstructProblem(pci); + } + + TrajOptProbPtr cppMethod() + { + ProblemConstructionInfo pci(env_); + + // Populate Basic Info + pci.basic_info.n_steps = steps_; + pci.basic_info.manip = "manipulator"; + pci.basic_info.start_fixed = false; + // pci.basic_info.dofs_fixed + + // Create Kinematic Object + pci.kin = pci.env->getManipulator(pci.basic_info.manip); + + // Populate Init Info + Eigen::VectorXd start_pos = pci.env->getCurrentJointValues(pci.kin->getName()); + + pci.init_info.type = InitInfo::STATIONARY; + pci.init_info.data = start_pos.transpose().replicate(pci.basic_info.n_steps, 1); + + // Populate Cost Info + std::shared_ptr jv = std::shared_ptr(new JointVelCostInfo); + jv->coeffs = std::vector(7, 5.0); + jv->name = "joint_vel"; + jv->term_type = TT_COST; + pci.cost_infos.push_back(jv); + + std::shared_ptr collision = std::shared_ptr(new CollisionCostInfo); + collision->name = "collision"; + collision->term_type = TT_COST; + collision->continuous = false; + collision->first_step = 0; + collision->last_step = pci.basic_info.n_steps - 1; + collision->gap = 1; + collision->info = createSafetyMarginDataVector(pci.basic_info.n_steps, 0.025, 20); + pci.cost_infos.push_back(collision); + + // Populate Constraints + double delta = 0.5/pci.basic_info.n_steps; + for (auto i = 0; i < pci.basic_info.n_steps; ++i) + { + Vector3d xyz(0.5, -0.2 + delta * i, 0.62); + Vector4d wxyz(0.0, 0.0, 1.0, 0.0); + + Affine3d cur_pose; + cur_pose.translation() = xyz; + Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + cur_pose.linear() = q.matrix(); + pose_inverses_.insert(pose_inverses_.begin() + i, cur_pose.inverse()); + + if (constraint_type_ == "confined") { + std::shared_ptr pose(new ConfinedAxisTermInfo); + pose->tol = tol_; + pose->axis = 'y'; + pose->term_type = TT_CNT; + pose->name = "waypoint_cart_" + std::to_string(i); + pose->link = "tool0"; + pose->timestep = i; + pose->xyz = Eigen::Vector3d(0.5, -0.2 + delta * i, 0.62); + pose->wxyz = Eigen::Vector4d(0.0, 0.0, 1.0, 0.0); + pose->pos_coeffs = Eigen::Vector3d(10, 10, 10); + pose->axis_coeff = 10.0; + pose->confined_coeff = 10.0; + pci.cnt_infos.push_back(pose); + } + else if (constraint_type_ == "conical") { + std::shared_ptr pose(new ConicalAxisTermInfo); + pose->tol = tol_; + pose->axis = 'z'; + pose->term_type = TT_CNT; + pose->name = "waypoint_cart_" + std::to_string(i); + pose->link = "tool0"; + pose->timestep = i; + pose->xyz = Eigen::Vector3d(0.5, -0.2 + delta * i, 0.62); + pose->wxyz = Eigen::Vector4d(0.0, 0.0, 1.0, 0.0); + pose->pos_coeffs = Eigen::Vector3d(10, 10, 10); + pose->axis_coeff = 10.0; + pose->conical_coeff = 10.0; + pci.cnt_infos.push_back(pose); + } + else { + ROS_ERROR("%s is not a valid constraint type. Exiting", constraint_type_.c_str()); + exit(-1); + } + + } + + return ConstructProblem(pci); + } + + virtual void SetUp() + { + + // Initial setup + std::string urdf_xml_string, srdf_xml_string; + nh_.getParam(ROBOT_DESCRIPTION_PARAM, urdf_xml_string); + nh_.getParam(ROBOT_SEMANTIC_PARAM, srdf_xml_string); + urdf_model_ = urdf::parseURDF(urdf_xml_string); + + srdf_model_ = srdf::ModelSharedPtr(new srdf::Model); + srdf_model_->initString(*urdf_model_, srdf_xml_string); + env_ = tesseract_ros::KDLEnvPtr(new tesseract_ros::KDLEnv); + plotter_ = tesseract_ros::ROSBasicPlottingPtr(new tesseract_ros::ROSBasicPlotting(env_)); + assert(env_ != nullptr); + + bool success = env_->init(urdf_model_, srdf_model_); + assert(success); + + pcl::PointCloud full_cloud; + double delta = 0.05; + int length = (1/delta); + + for (int x = 0; x < length; ++x) + for (int y = 0; y < length; ++y) + for (int z = 0; z < length; ++z) + full_cloud.push_back(pcl::PointXYZ(-0.5 + x*delta, -0.5 + y*delta, -0.5 + z*delta)); + + sensor_msgs::PointCloud2 pointcloud_msg; + pcl::toROSMsg(full_cloud, pointcloud_msg); + + octomap::Pointcloud octomap_data; + octomap::pointCloud2ToOctomap(pointcloud_msg, octomap_data); + octomap::OcTree* octree = new octomap::OcTree(2*delta); + octree->insertPointCloud(octomap_data, octomap::point3d(0,0,0)); + + AttachableObjectPtr obj(new AttachableObject()); + shapes::OcTree* octomap_world = new shapes::OcTree(std::shared_ptr(octree)); + Eigen::Affine3d octomap_pose; + + octomap_pose.setIdentity(); + octomap_pose.translation() = Eigen::Vector3d(1, 0, 0); + + obj->name = "octomap_attached"; + obj->collision.shapes.push_back(shapes::ShapeConstPtr(octomap_world)); + obj->collision.shape_poses.push_back(octomap_pose); + obj->collision.collision_object_types.push_back(CollisionObjectType::UseShapeType); + env_->addAttachableObject(obj); + + AttachedBodyInfo attached_body; + attached_body.object_name = "octomap_attached"; + attached_body.parent_link_name = "base_link"; + attached_body.transform = Eigen::Affine3d::Identity(); + env_->attachBody(attached_body); + + // Set the robot initial state + std::unordered_map ipos; + ipos["joint_a1"] = -0.4; + ipos["joint_a2"] = 0.2762; + ipos["joint_a3"] = 0.0; + ipos["joint_a4"] = -1.3348; + ipos["joint_a5"] = 0.0; + ipos["joint_a6"] = 1.4959; + ipos["joint_a7"] = 0.0; + + env_->setState(ipos); + + plotter_->plotScene(); + + // Set Log Level + gLogLevel = util::LevelError; + + } +}; + +TEST_P(AngularConstraintTest, AngularConstraint) +{ + // Setup Problem + TrajOptProbPtr prob; + testInfo info = GetParam(); + std::string method = info.method; + constraint_type_ = info.constraint; + + int num_iterations = method == "cpp" ? 5 : 1; + for (int i = 0; i < num_iterations; i++) { + tol_ = method == "cpp" ? 2.5 * (i + 1) : 15.0; + + if (method == "cpp") { + prob = cppMethod(); + } + else if (method == "json") { + prob = jsonMethod(); + } + else { + ROS_ERROR("Method string invalid. Exiting"); + exit(-1); + } + + tesseract::ContactResultMap collisions; + const std::vector& joint_names = prob->GetKin()->getJointNames(); + const std::vector& link_names = prob->GetKin()->getLinkNames(); + + env_->continuousCollisionCheckTrajectory(joint_names, link_names, prob->GetInitTraj(), collisions); + + tesseract::ContactResultVector collision_vector; + tesseract::moveContactResultsMapToContactResultsVector(collisions, collision_vector); + ROS_INFO("Initial trajector number of continuous collisions: %lu\n", collision_vector.size()); + + BasicTrustRegionSQP opt(prob); + if (plotting_) + { + opt.addCallback(PlotCallback(*prob, plotter_)); + } + + opt.initialize(trajToDblVec(prob->GetInitTraj())); + ros::Time tStart = ros::Time::now(); + opt.optimize(); + ROS_INFO("planning time: %.3f", (ros::Time::now() - tStart).toSec()); + + if (plotting_) + { + plotter_->clear(); + } + + collisions.clear(); + env_->continuousCollisionCheckTrajectory(joint_names, link_names, getTraj(opt.x(), prob->GetVars()), collisions); + ROS_INFO("Final trajectory number of continuous collisions: %lu\n", collisions.size()); + + auto manip = prob->GetKin(); + auto change_base = env_->getLinkTransform(manip->getBaseLinkName()); + + Affine3d tcp; + tcp.setIdentity(); + + TrajArray traj = getTraj(opt.x(), prob->GetVars()); + for (auto j = 0; j < traj.rows(); j++) { + VectorXd joint_angles(traj.cols()); + for (auto k = 0; k < traj.cols(); k++) { + joint_angles(k) = traj(j,k); + } + + Affine3d pose; + manip->calcFwdKin(pose, change_base, joint_angles); + + Affine3d pose_inv = pose_inverses_.at(j); + Affine3d err = pose_inv * (pose * tcp); + + if (constraint_type_ == "confined") { + AngleAxisd aa(err.rotation()); + EXPECT_LE(aa.angle(), tol_ * M_PI/180.0 * (1.0 + 1e-4) ); + + Vector3d axis = aa.axis(); + double axis_x = axis(0); + double axis_y = axis(1); + double axis_z = axis(2); + EXPECT_NEAR(0.0, axis_x, 1e-2); + if (axis_y > 0) { + EXPECT_NEAR(1.0, axis_y, 1e-4); + } + else { + EXPECT_NEAR(-1.0, axis_y, 1e-2); + } + EXPECT_NEAR(0.0, axis_z, 1e-3); + + } + else { + Matrix3d orientation(err.rotation()); + double angle = acos(orientation(2,2)) * 180.0/M_PI; + EXPECT_LE(angle, tol_ * (1 + 1e-4)); + } + + Vector3d pos = err.translation(); + EXPECT_NEAR(0.0, pos(0), 1e-4); + EXPECT_NEAR(0.0, pos(1), 1e-4); + EXPECT_NEAR(0.0, pos(2), 1e-4); + } + + } + +} + +INSTANTIATE_TEST_CASE_P(Tests, AngularConstraintTest, ::testing::Values(testInfo("json", "confined"), testInfo("json", "conical"), + testInfo("cpp", "confined"), testInfo("cpp", "conical")) ); + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "angular_constraint_unit"); + ros::NodeHandle pnh("~"); + + // Get ROS Parameters + pnh.param("plotting", plotting_, plotting_); + + return RUN_ALL_TESTS(); + +} + + diff --git a/trajopt/test/angular_constraint_unit.launch b/trajopt/test/angular_constraint_unit.launch new file mode 100644 index 00000000..0d98edd5 --- /dev/null +++ b/trajopt/test/angular_constraint_unit.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/trajopt/test/basic_cartesian_plan_confined_axis.json b/trajopt/test/basic_cartesian_plan_confined_axis.json new file mode 100644 index 00000000..ac3c7234 --- /dev/null +++ b/trajopt/test/basic_cartesian_plan_confined_axis.json @@ -0,0 +1,114 @@ +{ + "basic_info" : + { + "n_steps" : 5, + "manip" : "manipulator", + "start_fixed" : false + }, + "costs" : + [ + { + "type" : "joint_vel", + "params": + { + "coeffs" : [5] + } + }, + { + "type" : "collision", + "params" : + { + "coeffs" : [20], + "dist_pen" : [0.025], + "continuous" : false + } + } + ], + "constraints" : + [ + { + "name" : "waypoint_cart_1", + "type" : "confined_axis", + "params" : + { + "timestep" : 0, + "xyz" : [0.5, -0.2, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "axis_coeff" : 10.0, + "confined_coeff" : 10.0, + "axis" : "y", + "tolerance" : 15.0 + } + }, + { + "name" : "waypoint_cart_2", + "type" : "confined_axis", + "params" : + { + "timestep" : 1, + "xyz" : [0.5, -0.1, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "axis_coeff" : 10.0, + "confined_coeff" : 10.0, + "axis" : "y", + "tolerance" : 15.0 + } + }, + { + "name" : "waypoint_cart_3", + "type" : "confined_axis", + "params" : + { + "timestep" : 2, + "xyz" : [0.5, 0.0, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "axis_coeff" : 10.0, + "confined_coeff" : 10.0, + "axis" : "y", + "tolerance" : 15.0 + } + }, + { + "name" : "waypoint_cart_4", + "type" : "confined_axis", + "params" : + { + "timestep" : 3, + "xyz" : [0.5, 0.1, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "axis_coeff" : 10.0, + "confined_coeff" : 10.0, + "axis" : "y", + "tolerance" : 15.0 + } + }, + { + "name" : "waypoint_cart_5", + "type" : "confined_axis", + "params" : + { + "timestep" : 4, + "xyz" : [0.5, 0.2, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "axis_coeff" : 10.0, + "confined_coeff" : 10.0, + "axis" : "y", + "tolerance" : 15.0 + } + } + ], + "init_info" : + { + "type" : "stationary" + } +} diff --git a/trajopt/test/basic_cartesian_plan_conical_axis.json b/trajopt/test/basic_cartesian_plan_conical_axis.json new file mode 100644 index 00000000..e7c896c4 --- /dev/null +++ b/trajopt/test/basic_cartesian_plan_conical_axis.json @@ -0,0 +1,114 @@ +{ + "basic_info" : + { + "n_steps" : 5, + "manip" : "manipulator", + "start_fixed" : false + }, + "costs" : + [ + { + "type" : "joint_vel", + "params": + { + "coeffs" : [5] + } + }, + { + "type" : "collision", + "params" : + { + "coeffs" : [20], + "dist_pen" : [0.025], + "continuous" : false + } + } + ], + "constraints" : + [ + { + "name" : "waypoint_cart_1", + "type" : "conical_axis", + "params" : + { + "timestep" : 0, + "xyz" : [0.5, -0.2, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "axis_coeff" : 10.0, + "conical_coeff" : 10.0, + "axis" : "z", + "tolerance" : 15.0 + } + }, + { + "name" : "waypoint_cart_2", + "type" : "conical_axis", + "params" : + { + "timestep" : 1, + "xyz" : [0.5, -0.1, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "axis_coeff" : 10.0, + "conical_coeff" : 10.0, + "axis" : "z", + "tolerance" : 15.0 + } + }, + { + "name" : "waypoint_cart_3", + "type" : "conical_axis", + "params" : + { + "timestep" : 2, + "xyz" : [0.5, 0.0, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "axis_coeff" : 10.0, + "conical_coeff" : 10.0, + "axis" : "z", + "tolerance" : 15.0 + } + }, + { + "name" : "waypoint_cart_4", + "type" : "conical_axis", + "params" : + { + "timestep" : 3, + "xyz" : [0.5, 0.1, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "axis_coeff" : 10.0, + "conical_coeff" : 10.0, + "axis" : "z", + "tolerance" : 15.0 + } + }, + { + "name" : "waypoint_cart_5", + "type" : "conical_axis", + "params" : + { + "timestep" : 4, + "xyz" : [0.5, 0.2, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "axis_coeff" : 10.0, + "conical_coeff" : 10.0, + "axis" : "z", + "tolerance" : 15.0 + } + } + ], + "init_info" : + { + "type" : "stationary" + } +} diff --git a/trajopt/test/cast_cost_attached_unit.cpp b/trajopt/test/cast_cost_attached_unit.cpp index 4fc735ef..e88d0ae7 100644 --- a/trajopt/test/cast_cost_attached_unit.cpp +++ b/trajopt/test/cast_cost_attached_unit.cpp @@ -200,7 +200,6 @@ int main(int argc, char** argv) testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "trajopt_cast_cost_attached_unit"); ros::NodeHandle pnh("~"); - pnh.param("plotting", plotting, false); return RUN_ALL_TESTS(); } diff --git a/trajopt_examples/CMakeLists.txt b/trajopt_examples/CMakeLists.txt index ce2d3c32..b1729d95 100644 --- a/trajopt_examples/CMakeLists.txt +++ b/trajopt_examples/CMakeLists.txt @@ -26,6 +26,12 @@ include_directories( SYSTEM ${PCL_INCLUDE_DIRS} ) +#add_executable(${PROJECT_NAME}_basic_cartesian_plan_conical_axis src/basic_cartesian_plan_conical_axis.cpp) +#target_link_libraries(${PROJECT_NAME}_basic_cartesian_plan_conical_axis ${PCL_LIBRARIES} ${catkin_LIBRARIES} ) + +#add_executable(${PROJECT_NAME}_basic_cartesian_plan_confined_axis src/basic_cartesian_plan_confined_axis.cpp) +#target_link_libraries(${PROJECT_NAME}_basic_cartesian_plan_confined_axis ${PCL_LIBRARIES} ${catkin_LIBRARIES} ) + add_executable(${PROJECT_NAME}_basic_cartesian_plan src/basic_cartesian_plan.cpp) target_link_libraries(${PROJECT_NAME}_basic_cartesian_plan ${PCL_LIBRARIES} ${catkin_LIBRARIES} ) diff --git a/trajopt_examples/src/basic_cartesian_plan.cpp b/trajopt_examples/src/basic_cartesian_plan.cpp index 75ef88dd..b116ca51 100644 --- a/trajopt_examples/src/basic_cartesian_plan.cpp +++ b/trajopt_examples/src/basic_cartesian_plan.cpp @@ -110,6 +110,7 @@ TrajOptProbPtr cppMethod() collision->last_step = pci.basic_info.n_steps - 1; collision->gap = 1; collision->info = createSafetyMarginDataVector(pci.basic_info.n_steps, 0.025, 20); + pci.cost_infos.push_back(collision); // Populate Constraints double delta = 0.5 / pci.basic_info.n_steps; @@ -230,7 +231,7 @@ int main(int argc, char** argv) tesseract::ContactResultVector collision_vector; tesseract::moveContactResultsMapToContactResultsVector(collisions, collision_vector); - ROS_INFO("Initial trajector number of continuous collisions: %lui\n", collision_vector.size()); + ROS_INFO("Initial trajector number of continuous collisions: %lu\n", collision_vector.size()); BasicTrustRegionSQP opt(prob); if (plotting_) @@ -250,5 +251,5 @@ int main(int argc, char** argv) collisions.clear(); env_->continuousCollisionCheckTrajectory(joint_names, link_names, getTraj(opt.x(), prob->GetVars()), collisions); - ROS_INFO("Final trajectory number of continuous collisions: %lui\n", collisions.size()); + ROS_INFO("Final trajectory number of continuous collisions: %lu\n", collisions.size()); } From 98c88283a2eecd453cbf65f58d25018159d7f66f Mon Sep 17 00:00:00 2001 From: Reid Christopher Date: Fri, 10 Aug 2018 11:27:15 -0500 Subject: [PATCH 2/4] Refactoring of info terms and calculators and addition of axis enums. Removed trajectory file writing items to be placed in a separate branch. --- trajopt/CMakeLists.txt | 1 - .../include/trajopt/file_write_callback.hpp | 26 --- trajopt/include/trajopt/json_marshal.hpp | 1 - trajopt/include/trajopt/kinematic_terms.hpp | 29 ++-- .../include/trajopt/problem_description.hpp | 5 +- trajopt/scripts/traj_graph.py | 162 ------------------ trajopt/src/file_write_callback.cpp | 143 ---------------- trajopt/src/json_marshal.cpp | 4 - trajopt/src/kinematic_terms.cpp | 17 +- trajopt/src/problem_description.cpp | 75 +++++--- .../basic_cartesian_plan_confined_axis.json | 10 +- .../basic_cartesian_plan_conical_axis.json | 10 +- trajopt_examples/CMakeLists.txt | 6 - 13 files changed, 88 insertions(+), 401 deletions(-) delete mode 100644 trajopt/include/trajopt/file_write_callback.hpp delete mode 100644 trajopt/scripts/traj_graph.py delete mode 100644 trajopt/src/file_write_callback.cpp diff --git a/trajopt/CMakeLists.txt b/trajopt/CMakeLists.txt index 1b4ef271..d488e478 100644 --- a/trajopt/CMakeLists.txt +++ b/trajopt/CMakeLists.txt @@ -29,7 +29,6 @@ set(TRAJOPT_SOURCE_FILES src/problem_description.cpp src/utils.cpp src/plot_callback.cpp - src/file_write_callback.cpp ) catkin_package( diff --git a/trajopt/include/trajopt/file_write_callback.hpp b/trajopt/include/trajopt/file_write_callback.hpp deleted file mode 100644 index cabbde65..00000000 --- a/trajopt/include/trajopt/file_write_callback.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -typedef std::shared_ptr ofstreamPtr; - -using namespace Eigen; -using namespace util; -using namespace std; -namespace trajopt -{ - - Optimizer::Callback WriteCallback(string file_name,TrajOptProbPtr prob, vector joint_names, - vector pose_inverses, Affine3d tcp = Affine3d::Identity(), bool angle_axis = true, bool degrees = true); - -} diff --git a/trajopt/include/trajopt/json_marshal.hpp b/trajopt/include/trajopt/json_marshal.hpp index 4eb8510b..c74915f6 100644 --- a/trajopt/include/trajopt/json_marshal.hpp +++ b/trajopt/include/trajopt/json_marshal.hpp @@ -12,7 +12,6 @@ namespace json_marshal TRAJOPT_API void fromJson(const Json::Value& v, bool& ref); TRAJOPT_API void fromJson(const Json::Value& v, int& ref); TRAJOPT_API void fromJson(const Json::Value& v, double& ref); -TRAJOPT_API void fromJson(const Json::Value& v, char& ref); TRAJOPT_API void fromJson(const Json::Value& v, std::string& ref); TRAJOPT_API void fromJson(const Json::Value& v, Eigen::Vector3d& ref); TRAJOPT_API void fromJson(const Json::Value& v, Eigen::Vector4d& ref); diff --git a/trajopt/include/trajopt/kinematic_terms.hpp b/trajopt/include/trajopt/kinematic_terms.hpp index 8c7abd25..4ea4842b 100644 --- a/trajopt/include/trajopt/kinematic_terms.hpp +++ b/trajopt/include/trajopt/kinematic_terms.hpp @@ -1,5 +1,4 @@ #pragma once - #include #include #include @@ -105,6 +104,12 @@ struct CartVelCalculator : VectorOfVector VectorXd operator()(const VectorXd& dof_vals) const; }; +enum Axis { + X_AXIS, + Y_AXIS, + Z_AXIS +}; + /** * @brief The ConfinedAxisErrCalculator is a truct whose operator() calculates error of a given * pose with respect ot the confined rotation along the defined axis. @@ -117,9 +122,8 @@ struct ConfinedAxisErrCalculator : public VectorOfVector { std::string link_; /**< Link of the robot referred to */ Eigen::Affine3d tcp_; /**< Tool center point */ - char axis_; /**< Axis of rotation being provided with a tolerance */ - double tol_; /**< Tolerance of the rotation stored specific to the error calculation - NOTE: this is not an angle, but a value used in error calculation stored to avoid conversion using the angle during each call */ + Axis axis_; /**< Axis of rotation being provided with a tolerance */ + double tol_; /**< Tolerance angle in radians */ /** * @brief ConfinedAxisErrCalculator Constructor @@ -132,15 +136,15 @@ struct ConfinedAxisErrCalculator : public VectorOfVector { * @param tcp Tool center point */ ConfinedAxisErrCalculator(const Eigen::Affine3d& pose, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env, - std::string link, char axis, double tol_angle, Eigen::Affine3d tcp = Eigen::Affine3d::Identity()) : + std::string link, Axis axis, double tol_angle, Eigen::Affine3d tcp = Eigen::Affine3d::Identity()) : pose_inv_(pose.inverse()), manip_(manip), env_(env), link_(link), tcp_(tcp), - axis_(axis) + axis_(axis), + tol_(tol_angle * M_PI / 180.0) { - tol_ = sin(tol_angle*M_PI/180.0 / 2.0); } /** @@ -164,9 +168,8 @@ struct ConicalAxisErrCalculator : public VectorOfVector { std::string link_; /**< The link of the robot reggered to */ Eigen::Affine3d tcp_; /**< Tool center point */ - char axis_; /**< Axis the conical tolreance is applied to */ - double tol_; /**< Internal representation of the tolerance. - NOTE: this is not an angle, but a value used in error calculation stored to avoid conversion using the angle during each call */ + Axis axis_; /**< Axis the conical tolreance is applied to */ + double tol_; /**< Tolerance angle in radians */ /** * @brief ConicalAxisErrCalculator @@ -179,15 +182,15 @@ struct ConicalAxisErrCalculator : public VectorOfVector { * @param tcp Tool center point */ ConicalAxisErrCalculator(const Eigen::Affine3d& pose, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env, - std::string link, char axis, double tol_angle, Eigen::Affine3d tcp = Eigen::Affine3d::Identity()) : + std::string link, Axis axis, double tol_angle, Eigen::Affine3d tcp = Eigen::Affine3d::Identity()) : pose_inv_(pose.inverse()), manip_(manip), env_(env), link_(link), tcp_(tcp), - axis_(axis) + axis_(axis), + tol_(tol_angle * M_PI / 180.0) { - tol_ = cos(tol_angle*M_PI/180.0); } /** diff --git a/trajopt/include/trajopt/problem_description.hpp b/trajopt/include/trajopt/problem_description.hpp index 002bc993..4cb909f5 100644 --- a/trajopt/include/trajopt/problem_description.hpp +++ b/trajopt/include/trajopt/problem_description.hpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace sco { @@ -339,7 +340,7 @@ struct ConfinedAxisTermInfo : public TermInfo, public MakesConstraint, public Ma string link; /**< Link of the robot the term refers to */ Eigen::Affine3d tcp; /**< Tool center point */ - char axis; /**< Axis allowed to rotate */ + Axis axis; /**< Axis allowed to rotate */ double tol; /**< Rotation acceptable in degrees */ ConfinedAxisTermInfo(); @@ -372,7 +373,7 @@ struct ConicalAxisTermInfo : public TermInfo, public MakesConstraint, public Mak string link; /**< Link of the robot the term refers to */ Eigen::Affine3d tcp; /**< Tool center point */ - char axis; /**< Axis given a conical tolerance */ + Axis axis; /**< Axis given a conical tolerance */ double tol; /**< Cone angle in degrees */ ConicalAxisTermInfo(); diff --git a/trajopt/scripts/traj_graph.py b/trajopt/scripts/traj_graph.py deleted file mode 100644 index 40fbf39c..00000000 --- a/trajopt/scripts/traj_graph.py +++ /dev/null @@ -1,162 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import argparse -import pandas as pd - -# parse file names from command line -parser = argparse.ArgumentParser(description="Plot before and after trajectory") -parser.add_argument('a', type=str, help='First file') -parser.add_argument('b', type=str, help='Second file') -args = parser.parse_args() - -# read the files -data_a = pd.read_csv(args.a, skip_blank_lines=False) -data_b = pd.read_csv(args.b, skip_blank_lines=False) - -# get the dimensions of each of the files -shape_a = data_a.shape -rows_a = shape_a[0] -cols_a = shape_a[1] - -shape_b = data_b.shape -rows_b = shape_b[0] -cols_b = shape_b[1] - -# check that the files have the same number of columns -if (cols_a != cols_b): - print('The files do not have the same number of columns. Exiting'); - -# check that the files' columns hold the same information -if ((data_a.columns != data_b.columns).any()): - print('The column names for these files do not match. Exiting') - exit(-1) - -num_dofs = cols_a - 14 -cols = cols_a - -# check that the robots have the same number of poses set for the path -num_poses_a = 0 -num_poses_b = 0 - -var = data_a.iloc[0,0] -while (not np.isnan(var)): - num_poses_a += 1 - var = data_a.iloc[num_poses_a, 0] - -var = data_b.iloc[0,0] -while (not np.isnan(var)): - num_poses_b += 1 - var = data_b.iloc[num_poses_b, 0] - -if (num_poses_a != num_poses_b): - print('These two files do not describe the same path. Exiting') - exit(-1) - -# get the data for the final trajectories in each file -lower_a = rows_a - 1 - num_poses_a; -upper_a = rows_a - 1; -dof_vals_a = np.matrix(data_a.iloc[lower_a:upper_a, 0:num_dofs]) -pose_vals_a = np.matrix(data_a.iloc[lower_a:upper_a, num_dofs:(num_dofs + 7)]) -err_vals_a = np.matrix(data_a.iloc[lower_a:upper_a, (num_dofs + 7):(num_dofs + 14)]) - -lower_b = rows_b - 1 - num_poses_b; -upper_b = rows_b - 1; -dof_vals_b = np.matrix(data_b.iloc[lower_b:upper_b, 0:num_dofs]) -pose_vals_b = np.matrix(data_b.iloc[lower_b:upper_b, num_dofs:(num_dofs + 7)]) -err_vals_b = np.matrix(data_b.iloc[lower_b:upper_b, (num_dofs + 7):(num_dofs + 14)]) - -x = np.transpose(np.array(range(0,num_poses_a), ndmin=2)) - -# start at figure 1 and plot the DOFs -current_fig = 1 -plots_left = num_dofs - -# plot DOF values -for iter in range(0, (num_dofs >> 2) + 1): - - # open a new figure and determine the number of subplots - plt.figure(current_fig) - num_to_plot = min(plots_left, 4) - - # plots the subplots for the figure - for i in range(0, num_to_plot): - index = i + iter * 4 - cur_plot = plt.subplot(num_to_plot, 1, i + 1) - plt.plot(x, dof_vals_a[:, index], x, dof_vals_b[:, index]) - - dof_name = data_a.columns[i] - plt.legend([dof_name + '_' + args.a[:-4], dof_name + '_' + args.b[:-4]]) - - ylim = cur_plot.get_ylim() - diff = (ylim[1] - ylim[0]) * 0.1 - cur_plot.set_ylim([ylim[0] - diff, ylim[1] + diff]) - - # set the plot to fullscreen - #figManager = plt.get_current_fig_manager() - #figManager.full_screen_toggle() - - # increment the figure number and note the plots we finished - current_fig += 1 - plots_left -= 4 - -plots_left = 7 - -# plot pose values -for iter in range(0, 2): - - # open a new figure and determine the number of subplots - plt.figure(current_fig) - num_to_plot = min(plots_left, 4) - - # plots the subplots for the figure - for i in range(0, num_to_plot): - index = i + iter * 4 - cur_plot = plt.subplot(num_to_plot, 1, i + 1) - plt.plot(x, pose_vals_a[:, index], x, pose_vals_b[:, index]) - - pose_name = data_a.columns[index + num_dofs] - plt.legend([pose_name + '_' + args.a[:-4], pose_name + '_' + args.b[:-4]]) - - ylim = cur_plot.get_ylim() - diff = (ylim[1] - ylim[0]) * 0.1 - cur_plot.set_ylim([ylim[0] - diff, ylim[1] + diff]) - - # set the plot to fullscreen - #figManager = plt.get_current_fig_manager() - #figManager.full_screen_toggle(); - - # increment the figure number and note the plots we finished - current_fig += 1 - plots_left -= 4 - -plots_left = 7 - -# plot err values -for iter in range(0, 2): - - # open a new figure and determine the number of subplots - plt.figure(current_fig) - num_to_plot = min(plots_left, 4) - - # plots the subplots for the figure - for i in range(0, num_to_plot): - index = i + iter * 4 - cur_plot = plt.subplot(num_to_plot, 1, i + 1) - plt.plot(x, err_vals_a[:, index], x, err_vals_b[:, index]) - - err_name = data_a.columns[index + 7 + num_dofs] - plt.legend([err_name + '_' + args.a[:-4], err_name + '_' + args.b[:-4]]) - - ylim = cur_plot.get_ylim() - diff = (ylim[1] - ylim[0]) * 0.1 - cur_plot.set_ylim([ylim[0] - diff, ylim[1] + diff]) - - # set the plot to fullscreen - #figManager = plt.get_current_fig_manager() - #figManager.full_screen_toggle() - - # increment the figure number and note the plots we finished - current_fig += 1 - plots_left -= 4 - -plt.show() diff --git a/trajopt/src/file_write_callback.cpp b/trajopt/src/file_write_callback.cpp deleted file mode 100644 index b3001d72..00000000 --- a/trajopt/src/file_write_callback.cpp +++ /dev/null @@ -1,143 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace Eigen; -using namespace util; -using namespace std; -namespace trajopt -{ - - void WriteFile(ofstreamPtr file, vector pose_inverses, Affine3d tcp, TrajOptProbPtr prob, DblVec& x, - bool angle_axis, bool degrees) - { - // get joint info - //get pose info - //write to file - auto env = prob->GetEnv(); - auto manip = prob->GetKin(); - auto change_base = env->getLinkTransform(manip->getBaseLinkName()); - - TrajArray traj = getTraj(x, prob->GetVars()); - for (auto i = 0; i < traj.rows(); i++) { - VectorXd joint_angles(traj.cols()); - for (auto j = 0; j < traj.cols(); j++) { - if (j != 0) { - *file << ','; - } - *file << traj(i,j); - - joint_angles(j) = traj(i,j); - } - - Affine3d pose; - manip->calcFwdKin(pose, change_base, joint_angles); - - Vector4d rot_vec; - if (angle_axis) { - AngleAxisd aa(pose.rotation()); - Vector3d axis = aa.axis(); - rot_vec(0) = degrees ? aa.angle() * 180.0/M_PI : aa.angle(); - rot_vec(1) = axis(0); - rot_vec(2) = axis(1); - rot_vec(3) = axis(2); - - } - else { - Quaterniond q(pose.rotation()); - rot_vec(0) = q.w(); - rot_vec(1) = q.x(); - rot_vec(2) = q.y(); - rot_vec(3) = q.z(); - } - - VectorXd pose_vec = concat(pose.translation(), rot_vec); - for (auto i = 0;i < pose_vec.size(); i++) { - *file << ',' << pose_vec(i); - } - - Affine3d pose_inv = pose_inverses.at(i); - Affine3d err = pose_inv * (pose * tcp); - - Vector4d rot_err; - if (angle_axis) { - AngleAxisd aa(err.rotation()); - rot_err(0) = degrees ? aa.angle() * 180.0/M_PI : aa.angle(); - - Vector3d axis = aa.axis(); - rot_err(1) = axis(0); - rot_err(2) = axis(1); - rot_err(3) = axis(2); - } - else { - Quaterniond q(err.rotation()); - rot_err(0) = q.w(); - rot_err(1) = q.x(); - rot_err(2) = q.y(); - rot_err(3) = q.z(); - } - - VectorXd err_vec = concat(err.translation(), rot_err); - for (auto j = 0; j < err_vec.size(); j++) { - *file << "," << err_vec(j); - } - - *file << endl; - } - *file << endl; - } - - Optimizer::Callback WriteCallback(string file_name, TrajOptProbPtr prob, vector joint_names, vector pose_inverses, - Affine3d tcp /*=identity*/, bool angle_axis /*=true*/, bool degrees /*=true*/) - { - - ofstreamPtr file(new ofstream(file_name, ofstream::trunc)); - ROS_ERROR(file->good() ? "File successfully opened" : "Error opening file"); - - for (size_t i = 0; i < joint_names.size(); i++) { - if (i != 0) { - *file << ','; - } - *file << joint_names.at(i); - } - - vector pose_str = angle_axis ? vector{"x", "y", "z", "angle", "axis_x", "axis_y", "axis_z"} : - vector{"x", "y", "z", "q_w", "q_x", "q_y", "q_z"}; - - vector err_str(pose_str.size()); - for (size_t i = 0; i < err_str.size(); i++) { - err_str.at(i) = pose_str.at(i) + "_err"; - } - - for (size_t i = 0; i < pose_str.size(); i++) { - *file << ',' << pose_str.at(i); - } - - for (size_t i = 0; i < err_str.size(); i++) { - *file << ',' << err_str.at(i); - } - - *file << endl; - - // return callback function - return bind(&WriteFile, - file, - pose_inverses, - tcp, - prob, - placeholders::_2, - angle_axis, - degrees); - } - -} diff --git a/trajopt/src/json_marshal.cpp b/trajopt/src/json_marshal.cpp index 686ad552..67f52695 100644 --- a/trajopt/src/json_marshal.cpp +++ b/trajopt/src/json_marshal.cpp @@ -42,10 +42,6 @@ void fromJson(const Json::Value& v, double& ref) } } -void fromJson(const Json::Value& v, char& ref) { - ref = *(v.asCString()); -} - void fromJson(const Json::Value& v, std::string& ref) { try diff --git a/trajopt/src/kinematic_terms.cpp b/trajopt/src/kinematic_terms.cpp index db5039b6..0c690927 100644 --- a/trajopt/src/kinematic_terms.cpp +++ b/trajopt/src/kinematic_terms.cpp @@ -10,8 +10,6 @@ #include #include - -#include using namespace std; using namespace sco; using namespace Eigen; @@ -191,11 +189,11 @@ VectorXd ConfinedAxisErrCalculator::operator()(const VectorXd& dof_vals) const { // determine the error of the rotation switch(axis_) { - case 'x': err(0) = fabs(q.x()) - tol_; + case X_AXIS: err(0) = 2.0*fabs(asin(q.x())) - tol_; break; - case 'y': err(0) = fabs(q.y()) - tol_; + case Y_AXIS: err(0) = 2.0*fabs(asin(q.y())) - tol_; break; - case 'z': err(0) = fabs(q.z()) - tol_; + case Z_AXIS: err(0) = 2.0*fabs(asin(q.z())) - tol_; break; } @@ -212,17 +210,16 @@ VectorXd ConicalAxisErrCalculator::operator()(const VectorXd& dof_vals) const { Affine3d pose_err = pose_inv_ * (new_pose * tcp_); // get the orientation matrix of the error - Matrix3d orientation(pose_err.rotation()); + Matrix3d orientation_err(pose_err.rotation()); VectorXd err(1); // determine the error of the conical axis - // tol_ = cos(tol_angle), so when angle < tol_angle, err < 0, and vice versa switch(axis_) { - case 'x': err(0) = tol_ - orientation(0, 0); + case X_AXIS: err(0) = acos(orientation_err(0, 0)) - tol_; break; - case 'y': err(0) = tol_ - orientation(1, 1); + case Y_AXIS: err(0) = acos(orientation_err(1, 1)) - tol_; break; - case 'z': err(0) = tol_ - orientation(2, 2); + case Z_AXIS: err(0) = acos(orientation_err(2, 2)) - tol_; break; } diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp index c67a4a2f..125333a9 100644 --- a/trajopt/src/problem_description.cpp +++ b/trajopt/src/problem_description.cpp @@ -68,6 +68,27 @@ BoolVec toMask(const VectorXd& x) { return out; } #endif + +Axis stringToAxisEnum(string& str) +{ + if (str == "X_AXIS") + { + return X_AXIS; + } + else if (str == "Y_AXIS") + { + return Y_AXIS; + } + else if (str == "Z_AXIS") + { + return Z_AXIS; + } + else + { + PRINT_AND_THROW(boost::format("%s is not a valid axis string.") % str); + } + +} } namespace trajopt @@ -830,7 +851,11 @@ void ConfinedAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value& v childFromJson(params, axis_coeff, "axis_coeff", (double)1.0); childFromJson(params, confined_coeff, "confined_coeff", (double)1.0); childFromJson(params, link, "link"); - childFromJson(params, axis, "axis"); + + string axis_str; + childFromJson(params, axis_str, "axis"); + axis = stringToAxisEnum(axis_str); + childFromJson(params, tol, "tolerance"); // construct the tcp Affine matrix from its components @@ -874,18 +899,18 @@ void ConfinedAxisTermInfo::hatch(TrajOptProb& prob) { // the error corresponding to the axis of rotation having the proper orientation corresponds to the // values of the other 2 quaternion error components switch(axis) { - case 'x': - pose_coeffs(1) = axis_coeff; - pose_coeffs(2) = axis_coeff; - break; - case 'y': - pose_coeffs(0) = axis_coeff; - pose_coeffs(2) = axis_coeff; - break; - case 'z': - pose_coeffs(0) = axis_coeff; - pose_coeffs(1) = axis_coeff; - break; + case X_AXIS: + pose_coeffs(1) = axis_coeff; + pose_coeffs(2) = axis_coeff; + break; + case Y_AXIS: + pose_coeffs(0) = axis_coeff; + pose_coeffs(2) = axis_coeff; + break; + case Z_AXIS: + pose_coeffs(0) = axis_coeff; + pose_coeffs(1) = axis_coeff; + break; } // add the costs or constraints depending on the term type @@ -925,7 +950,11 @@ void ConicalAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value &v) childFromJson(params, axis_coeff, "axis_coeff", (double)1.0); childFromJson(params, conical_coeff, "conical_coeff", (double)1.0); childFromJson(params, link, "link"); - childFromJson(params, axis, "axis"); + + string axis_str; + childFromJson(params, axis_str, "axis"); + axis = stringToAxisEnum(axis_str); + childFromJson(params, tol, "tolerance"); // construct the tcp Affine matrix from its components @@ -969,15 +998,15 @@ void ConicalAxisTermInfo::hatch(TrajOptProb &prob) { // the error corresponding to the rotation about the axis in the cone is given by // the quaternion component of that axis switch(axis) { - case 'x': - pose_coeffs(0) = axis_coeff; - break; - case 'y': - pose_coeffs(1) = axis_coeff; - break; - case 'z': - pose_coeffs(2) = axis_coeff; - break; + case X_AXIS: + pose_coeffs(0) = axis_coeff; + break; + case Y_AXIS: + pose_coeffs(1) = axis_coeff; + break; + case Z_AXIS: + pose_coeffs(2) = axis_coeff; + break; } // add the costs or constraints depending on the term type diff --git a/trajopt/test/basic_cartesian_plan_confined_axis.json b/trajopt/test/basic_cartesian_plan_confined_axis.json index ac3c7234..594ab9ce 100644 --- a/trajopt/test/basic_cartesian_plan_confined_axis.json +++ b/trajopt/test/basic_cartesian_plan_confined_axis.json @@ -38,7 +38,7 @@ "pos_coeffs" : [10, 10, 10], "axis_coeff" : 10.0, "confined_coeff" : 10.0, - "axis" : "y", + "axis" : "Y_AXIS", "tolerance" : 15.0 } }, @@ -54,7 +54,7 @@ "pos_coeffs" : [10, 10, 10], "axis_coeff" : 10.0, "confined_coeff" : 10.0, - "axis" : "y", + "axis" : "Y_AXIS", "tolerance" : 15.0 } }, @@ -70,7 +70,7 @@ "pos_coeffs" : [10, 10, 10], "axis_coeff" : 10.0, "confined_coeff" : 10.0, - "axis" : "y", + "axis" : "Y_AXIS", "tolerance" : 15.0 } }, @@ -86,7 +86,7 @@ "pos_coeffs" : [10, 10, 10], "axis_coeff" : 10.0, "confined_coeff" : 10.0, - "axis" : "y", + "axis" : "Y_AXIS", "tolerance" : 15.0 } }, @@ -102,7 +102,7 @@ "pos_coeffs" : [10, 10, 10], "axis_coeff" : 10.0, "confined_coeff" : 10.0, - "axis" : "y", + "axis" : "Y_AXIS", "tolerance" : 15.0 } } diff --git a/trajopt/test/basic_cartesian_plan_conical_axis.json b/trajopt/test/basic_cartesian_plan_conical_axis.json index e7c896c4..bc0ebbfc 100644 --- a/trajopt/test/basic_cartesian_plan_conical_axis.json +++ b/trajopt/test/basic_cartesian_plan_conical_axis.json @@ -38,7 +38,7 @@ "pos_coeffs" : [10, 10, 10], "axis_coeff" : 10.0, "conical_coeff" : 10.0, - "axis" : "z", + "axis" : "Z_AXIS", "tolerance" : 15.0 } }, @@ -54,7 +54,7 @@ "pos_coeffs" : [10, 10, 10], "axis_coeff" : 10.0, "conical_coeff" : 10.0, - "axis" : "z", + "axis" : "Z_AXIS", "tolerance" : 15.0 } }, @@ -70,7 +70,7 @@ "pos_coeffs" : [10, 10, 10], "axis_coeff" : 10.0, "conical_coeff" : 10.0, - "axis" : "z", + "axis" : "Z_AXIS", "tolerance" : 15.0 } }, @@ -86,7 +86,7 @@ "pos_coeffs" : [10, 10, 10], "axis_coeff" : 10.0, "conical_coeff" : 10.0, - "axis" : "z", + "axis" : "Z_AXIS", "tolerance" : 15.0 } }, @@ -102,7 +102,7 @@ "pos_coeffs" : [10, 10, 10], "axis_coeff" : 10.0, "conical_coeff" : 10.0, - "axis" : "z", + "axis" : "Z_AXIS", "tolerance" : 15.0 } } diff --git a/trajopt_examples/CMakeLists.txt b/trajopt_examples/CMakeLists.txt index b1729d95..ce2d3c32 100644 --- a/trajopt_examples/CMakeLists.txt +++ b/trajopt_examples/CMakeLists.txt @@ -26,12 +26,6 @@ include_directories( SYSTEM ${PCL_INCLUDE_DIRS} ) -#add_executable(${PROJECT_NAME}_basic_cartesian_plan_conical_axis src/basic_cartesian_plan_conical_axis.cpp) -#target_link_libraries(${PROJECT_NAME}_basic_cartesian_plan_conical_axis ${PCL_LIBRARIES} ${catkin_LIBRARIES} ) - -#add_executable(${PROJECT_NAME}_basic_cartesian_plan_confined_axis src/basic_cartesian_plan_confined_axis.cpp) -#target_link_libraries(${PROJECT_NAME}_basic_cartesian_plan_confined_axis ${PCL_LIBRARIES} ${catkin_LIBRARIES} ) - add_executable(${PROJECT_NAME}_basic_cartesian_plan src/basic_cartesian_plan.cpp) target_link_libraries(${PROJECT_NAME}_basic_cartesian_plan ${PCL_LIBRARIES} ${catkin_LIBRARIES} ) From 8a6d6c499e25ee2ae2c704fadf5137f1c6aaa5ee Mon Sep 17 00:00:00 2001 From: Reid Christopher Date: Fri, 17 Aug 2018 17:44:55 -0500 Subject: [PATCH 3/4] Changing axis constraints to only orientation. Renamed ConfinedAxis to AlignedAxis. AlignedAxis is currently not interacting consistently with positional constraints --- trajopt/include/trajopt/kinematic_terms.hpp | 62 +++--- .../include/trajopt/problem_description.hpp | 50 ++--- trajopt/src/kinematic_terms.cpp | 41 ++-- trajopt/src/problem_description.cpp | 186 ++++-------------- trajopt/test/angular_constraint_unit.cpp | 131 ++++++------ trajopt/test/angular_constraint_unit.launch | 4 +- .../basic_cartesian_plan_aligned_axis.json | 169 ++++++++++++++++ .../basic_cartesian_plan_confined_axis.json | 114 ----------- .../basic_cartesian_plan_conical_axis.json | 100 +++++++--- 9 files changed, 410 insertions(+), 447 deletions(-) create mode 100644 trajopt/test/basic_cartesian_plan_aligned_axis.json delete mode 100644 trajopt/test/basic_cartesian_plan_confined_axis.json diff --git a/trajopt/include/trajopt/kinematic_terms.hpp b/trajopt/include/trajopt/kinematic_terms.hpp index 4ea4842b..dc1d0d75 100644 --- a/trajopt/include/trajopt/kinematic_terms.hpp +++ b/trajopt/include/trajopt/kinematic_terms.hpp @@ -104,29 +104,23 @@ struct CartVelCalculator : VectorOfVector VectorXd operator()(const VectorXd& dof_vals) const; }; -enum Axis { - X_AXIS, - Y_AXIS, - Z_AXIS -}; - /** - * @brief The ConfinedAxisErrCalculator is a truct whose operator() calculates error of a given - * pose with respect ot the confined rotation along the defined axis. + * @brief The AlignedAxisErrCalculator is a struct whose operator() calculates error of a given + * pose with respect to the rotation along the defined axis. */ -struct ConfinedAxisErrCalculator : public VectorOfVector { +struct AlignedAxisErrCalculator : public VectorOfVector { - Eigen::Affine3d pose_inv_; /**< param The inverse of the desired pose */ - tesseract::BasicKinConstPtr manip_; /**< Kinematics object */ - tesseract::BasicEnvConstPtr env_; /**< Environment object */ - std::string link_; /**< Link of the robot referred to */ - Eigen::Affine3d tcp_; /**< Tool center point */ + Eigen::Matrix3d orientation_inv_; /**< @brief param The inverse of the desired orientation */ + tesseract::BasicKinConstPtr manip_; /**< @brief Kinematics object */ + tesseract::BasicEnvConstPtr env_; /**< @brief Environment object */ + std::string link_; /**< @brief Link of the robot referred to */ + Eigen::Matrix3d tcp_orientation_; /**< @brief Tool center point orientation */ - Axis axis_; /**< Axis of rotation being provided with a tolerance */ - double tol_; /**< Tolerance angle in radians */ + Vector3d axis_; /**< @brief Axis of rotation to align with */ + double tol_; /**< @brief Tolerance angle in radians */ /** - * @brief ConfinedAxisErrCalculator Constructor + * @brief AlignedAxisErrCalculator Constructor * @param pose Desired pose * @param manip * @param env @@ -135,15 +129,15 @@ struct ConfinedAxisErrCalculator : public VectorOfVector { * @param tol_angle Tolerance in radians * @param tcp Tool center point */ - ConfinedAxisErrCalculator(const Eigen::Affine3d& pose, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env, - std::string link, Axis axis, double tol_angle, Eigen::Affine3d tcp = Eigen::Affine3d::Identity()) : - pose_inv_(pose.inverse()), + AlignedAxisErrCalculator(const Eigen::Matrix3d& orientation, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env, + std::string link, Vector3d axis, double tol, Eigen::Matrix3d tcp_orientation = Eigen::Matrix3d::Identity()) : + orientation_inv_(orientation.inverse()), manip_(manip), env_(env), link_(link), - tcp_(tcp), + tcp_orientation_(tcp_orientation), axis_(axis), - tol_(tol_angle * M_PI / 180.0) + tol_(tol) { } @@ -162,14 +156,14 @@ struct ConfinedAxisErrCalculator : public VectorOfVector { * */ struct ConicalAxisErrCalculator : public VectorOfVector { - Eigen::Affine3d pose_inv_; /**< Inverse of the desired pose */ - tesseract::BasicKinConstPtr manip_; /**< Kinematics object */ - tesseract::BasicEnvConstPtr env_; /**< Environment object */ - std::string link_; /**< The link of the robot reggered to */ - Eigen::Affine3d tcp_; /**< Tool center point */ + Eigen::Matrix3d orientation_inv_; /**< @brief Inverse of the desired orientation */ + tesseract::BasicKinConstPtr manip_; /**< @brief Kinematics object */ + tesseract::BasicEnvConstPtr env_; /**< @brief Environment object */ + std::string link_; /**< @brief The link of the robot referred to */ + Eigen::Matrix3d tcp_orientation_; /**< @brief Tool center point orientation */ - Axis axis_; /**< Axis the conical tolreance is applied to */ - double tol_; /**< Tolerance angle in radians */ + Vector3d axis_; /**< @brief Axis the conical tolerance is applied to */ + double tol_; /**< @brief Tolerance angle in radians */ /** * @brief ConicalAxisErrCalculator @@ -181,15 +175,15 @@ struct ConicalAxisErrCalculator : public VectorOfVector { * @param tol_angle Tolerance angle in degrees * @param tcp Tool center point */ - ConicalAxisErrCalculator(const Eigen::Affine3d& pose, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env, - std::string link, Axis axis, double tol_angle, Eigen::Affine3d tcp = Eigen::Affine3d::Identity()) : - pose_inv_(pose.inverse()), + ConicalAxisErrCalculator(const Eigen::Matrix3d& orientation, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env, + std::string link, Vector3d axis, double tol, Eigen::Matrix3d tcp_orientation = Eigen::Matrix3d::Identity()) : + orientation_inv_(orientation.inverse()), manip_(manip), env_(env), link_(link), - tcp_(tcp), + tcp_orientation_(tcp_orientation), axis_(axis), - tol_(tol_angle * M_PI / 180.0) + tol_(tol) { } diff --git a/trajopt/include/trajopt/problem_description.hpp b/trajopt/include/trajopt/problem_description.hpp index 4cb909f5..9cca6e87 100644 --- a/trajopt/include/trajopt/problem_description.hpp +++ b/trajopt/include/trajopt/problem_description.hpp @@ -326,24 +326,20 @@ struct JointConstraintInfo : public TermInfo, public MakesConstraint }; /** - * @brief The ConfinedAxisTermInfo struct contains information to create constraints or costs + * @brief The AlignedAxisTermInfo struct contains information to create constraints or costs * using a tolerance for the rotation about an axis */ -struct ConfinedAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost { - int timestep; /**< The timestep of this term in the trajectory */ - Vector3d xyz; /**< Cartesian position coordinate vector for the pose */ - Vector4d wxyz; /**< 4D vector containing w, x, y, and z quaternion components for the pose (in that order) */ - Vector3d pos_coeffs; /**< Coefficients multiplied by xyz errors during evaluation of cost/constraint values */ - double confined_coeff; /**< Coefficient multiplied by error of the rotation beyond tolerance */ - double axis_coeff; /**< Coefficient multiplied by the errors of the rotation axis from the specified axis */ - - string link; /**< Link of the robot the term refers to */ - Eigen::Affine3d tcp; /**< Tool center point */ - - Axis axis; /**< Axis allowed to rotate */ - double tol; /**< Rotation acceptable in degrees */ - - ConfinedAxisTermInfo(); +struct AlignedAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost { + int timestep; /**< @brief The timestep of this term in the trajectory */ + Vector4d wxyz; /**< @brief 4D vector containing w, x, y, and z quaternion components for the pose orientation (in that order) */ + Vector4d tcp_wxyz; /**< @brief Tool center point */ + double axis_coeff; /**< @brief Coefficient multiplied by the errors of the rotation axis from the specified axis */ + double angle_coeff; /**< @brief Coefficient multipleid by the angle of rotation past the tolerance */ + string link; /**< @brief Link of the robot the term refers to */ + Vector3d axis; /**< @brief Axis allowed to rotate with respect to the frame of #link */ + double tolerance; /**< @brief Rotation tolerance about the given axis in radians */ + + AlignedAxisTermInfo(); /** * @brief fromJson Constructs the term from a Json file @@ -355,7 +351,7 @@ struct ConfinedAxisTermInfo : public TermInfo, public MakesConstraint, public Ma * @param prob The optimization problems to add the term to */ void hatch(TrajOptProb& prob); - DEFINE_CREATE(ConfinedAxisTermInfo) + DEFINE_CREATE(AlignedAxisTermInfo) }; /** @@ -363,18 +359,14 @@ struct ConfinedAxisTermInfo : public TermInfo, public MakesConstraint, public Ma * using a conical tolerance about an axis */ struct ConicalAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost { - int timestep; /**< The timestep of this term in the trajectory */ - Vector3d xyz; /**< Cartesian position coordinate vector for the pose */ - Vector4d wxyz; /**< 4D vector containing w, x, y, and z quaternion components for the pose (in that order) */ - Vector3d pos_coeffs; /**< Coefficients multiplied by xyz errors during evaluation of cost/constraint values */ - double conical_coeff; /**< Coefficient multiplied by error of the axis orientation beyond tolerance */ - double axis_coeff; /**< Coefficient multiplied by the rotation error about the axis in the conical tolerance */ - - string link; /**< Link of the robot the term refers to */ - Eigen::Affine3d tcp; /**< Tool center point */ - - Axis axis; /**< Axis given a conical tolerance */ - double tol; /**< Cone angle in degrees */ + int timestep; /**< @brief The timestep of this term in the trajectory */ + Vector4d wxyz; /**< @brief 4D vector containing w, x, y, and z quaternion components for the pose orientation (in that order) */ + Vector4d tcp_wxyz; /**< @brief Tool center point */ + double weight; /**< @brief Coefficient multiplied by the errors of the rotation axis from the specified axis */ + string link; /**< @brief Link of the robot the term refers to */ + Vector3d axis; /**< @brief Axis around which the conical tolerance will be applied */ + double tolerance; /**< @brief Tolerance angle of the cone in radians*/ + ConicalAxisTermInfo(); diff --git a/trajopt/src/kinematic_terms.cpp b/trajopt/src/kinematic_terms.cpp index 0c690927..991bbe54 100644 --- a/trajopt/src/kinematic_terms.cpp +++ b/trajopt/src/kinematic_terms.cpp @@ -176,27 +176,28 @@ VectorXd CartVelCalculator::operator()(const VectorXd& dof_vals) const return out; } -VectorXd ConfinedAxisErrCalculator::operator()(const VectorXd& dof_vals) const { +VectorXd AlignedAxisErrCalculator::operator()(const VectorXd& dof_vals) const { // calculate the current pose given the DOF values for the robot Affine3d new_pose, change_base; change_base = env_->getLinkTransform(manip_->getBaseLinkName()); manip_->calcFwdKin(new_pose, change_base, dof_vals, link_, *env_->getState()); // get the error of the current pose - Affine3d pose_err = pose_inv_ * (new_pose * tcp_); - Quaterniond q(pose_err.rotation()); - VectorXd err(1); + Matrix3d orientation_err = orientation_inv_ * (new_pose.rotation() * tcp_orientation_); + AngleAxisd aa_err(orientation_err); + + // gets terms for error determination + double angle = fabs(aa_err.angle()); - // determine the error of the rotation - switch(axis_) { - case X_AXIS: err(0) = 2.0*fabs(asin(q.x())) - tol_; - break; - case Y_AXIS: err(0) = 2.0*fabs(asin(q.y())) - tol_; - break; - case Z_AXIS: err(0) = 2.0*fabs(asin(q.z())) - tol_; - break; - } + // the angle error is typical: actual value - tolerance + double angle_err = angle - tol_; + // the axis error is scaled with the angle. This is for two reasons: + // 1. The error terms will stay on the same scale, since 1 - |dot_prod| ranges from 0 to 1 + // 2. As the angle approaches zero, the axis loses meaning and should not be counted as erroneous. + Vector3d axis_err = (axis_ - (orientation_err*axis_)).array().square(); + + Vector4d err(axis_err.x(), axis_err.y(), axis_err.z(), angle_err); return err; } @@ -206,22 +207,12 @@ VectorXd ConicalAxisErrCalculator::operator()(const VectorXd& dof_vals) const { change_base = env_->getLinkTransform(manip_->getBaseLinkName()); manip_->calcFwdKin(new_pose, change_base, dof_vals, link_, *env_->getState()); - // get the error of the current pose - Affine3d pose_err = pose_inv_ * (new_pose * tcp_); - // get the orientation matrix of the error - Matrix3d orientation_err(pose_err.rotation()); + Matrix3d orientation_err = orientation_inv_ * (new_pose.rotation() * tcp_orientation_); VectorXd err(1); // determine the error of the conical axis - switch(axis_) { - case X_AXIS: err(0) = acos(orientation_err(0, 0)) - tol_; - break; - case Y_AXIS: err(0) = acos(orientation_err(1, 1)) - tol_; - break; - case Z_AXIS: err(0) = acos(orientation_err(2, 2)) - tol_; - break; - } + err(0) = acos((orientation_err*axis_).dot(axis_)) - tol_; return err; } diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp index 125333a9..c5289974 100644 --- a/trajopt/src/problem_description.cpp +++ b/trajopt/src/problem_description.cpp @@ -55,7 +55,7 @@ void RegisterMakers() TermInfo::RegisterMaker("joint", &JointConstraintInfo::create); TermInfo::RegisterMaker("cart_vel", &CartVelCntInfo::create); TermInfo::RegisterMaker("joint_vel_limits", &JointVelConstraintInfo::create); - TermInfo::RegisterMaker("confined_axis", &ConfinedAxisTermInfo::create); + TermInfo::RegisterMaker("aligned_axis", &AlignedAxisTermInfo::create); TermInfo::RegisterMaker("conical_axis", &ConicalAxisTermInfo::create); gRegisteredMakers = true; @@ -69,26 +69,6 @@ BoolVec toMask(const VectorXd& x) { } #endif -Axis stringToAxisEnum(string& str) -{ - if (str == "X_AXIS") - { - return X_AXIS; - } - else if (str == "Y_AXIS") - { - return Y_AXIS; - } - else if (str == "Z_AXIS") - { - return Z_AXIS; - } - else - { - PRINT_AND_THROW(boost::format("%s is not a valid axis string.") % str); - } - -} } namespace trajopt @@ -826,44 +806,26 @@ void JointConstraintInfo::hatch(TrajOptProb& prob) } // initialize with basic default values -ConfinedAxisTermInfo::ConfinedAxisTermInfo() { - pos_coeffs = Vector3d::Ones(); - axis_coeff = 1.0; - confined_coeff = 1.0; - tcp.setIdentity(); +AlignedAxisTermInfo::AlignedAxisTermInfo() : tcp_wxyz(1.0, 0.0, 0.0, 0.0) { + axis_coeff = 0.0; + angle_coeff = 0.0; } // get the term information from a json value -void ConfinedAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value& v) { +void AlignedAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value& v) { // make sure params is a member of the Json value provided FAIL_IF_FALSE(v.isMember("params")); - // provide default position and orienation for the tcp (tool center point) - Vector3d tcp_xyz = Vector3d::Zero(); - Vector4d tcp_wxyz = Vector4d(1, 0, 0, 0); - // get the params value and read the parameters fmro the file into the object members const Value& params = v["params"]; childFromJson(params, timestep, "timestep", pci.basic_info.n_steps-1); - childFromJson(params, xyz,"xyz"); - childFromJson(params, wxyz,"wxyz"); - childFromJson(params, pos_coeffs, "pos_coeffs", (Vector3d)Vector3d::Ones()); - childFromJson(params, axis_coeff, "axis_coeff", (double)1.0); - childFromJson(params, confined_coeff, "confined_coeff", (double)1.0); - childFromJson(params, link, "link"); - - string axis_str; - childFromJson(params, axis_str, "axis"); - axis = stringToAxisEnum(axis_str); - - childFromJson(params, tol, "tolerance"); - - // construct the tcp Affine matrix from its components - childFromJson(params, tcp_xyz, "tcp,xyz", tcp_xyz); + childFromJson(params, wxyz, "wxyz"); childFromJson(params, tcp_wxyz, "tcp_xwyz", tcp_wxyz); - Eigen::Quaterniond q(tcp_wxyz(0), tcp_wxyz(1), tcp_wxyz(2), tcp_wxyz(3)); - tcp.linear() = q.matrix(); - tcp.translation() = tcp_xyz; + childFromJson(params, axis_coeff, "axis_coeff"); + childFromJson(params, angle_coeff, "angle_coeff"); + childFromJson(params, link, "link"); + childFromJson(params, axis, "axis"); + childFromJson(params, tolerance, "tolerance"); // make sure the link name provided is valid const std::vector& link_names = pci.kin->getLinkNames(); @@ -872,64 +834,35 @@ void ConfinedAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value& v } // make sure there are no extra items in the params value - const char* all_fields[] = {"timestep", "xyz", "wxyz", "pos_coeffs", "axis_coeff", "confined_coeff", "link", - "tolerance", "axis", "tcp_xyz", "tcp_wxyz"}; + const char* all_fields[] = {"timestep", "wxyz", "axis_coeff", "angle_coeff", "link", + "tolerance", "axis", "tcp_wxyz"}; ensure_only_members(params, all_fields, sizeof(all_fields)/sizeof(char*)); } // add the term to the problem description -void ConfinedAxisTermInfo::hatch(TrajOptProb& prob) { +void AlignedAxisTermInfo::hatch(TrajOptProb& prob) { // construct the desired pose - Eigen::Affine3d input_pose; - Eigen::Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); - input_pose.linear() = q.matrix(); - input_pose.translation() = xyz; + Eigen::Quaterniond input_q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + Eigen::Quaterniond tcp_q(tcp_wxyz(0), tcp_wxyz(1), tcp_wxyz(2), tcp_wxyz(3)); // create the equality and inequality error functions to be used - VectorOfVectorPtr f_eq(new StaticCartPoseErrCalculator(input_pose, prob.GetKin(), prob.GetEnv(), link, tcp)); - VectorOfVectorPtr f_ineq(new ConfinedAxisErrCalculator(input_pose, prob.GetKin(), prob.GetEnv(), link, axis, tol, tcp)); - - // construct the coefficient vectors for the errors - VectorXd pose_coeffs = concat(Vector3d(0.0, 0.0, 0.0), pos_coeffs); - VectorXd ineq_coeffs(1); - - // the inequality coefficient is the one referrring to the error outside the confined rotation - ineq_coeffs(0) = confined_coeff; - - // the error corresponding to the axis of rotation having the proper orientation corresponds to the - // values of the other 2 quaternion error components - switch(axis) { - case X_AXIS: - pose_coeffs(1) = axis_coeff; - pose_coeffs(2) = axis_coeff; - break; - case Y_AXIS: - pose_coeffs(0) = axis_coeff; - pose_coeffs(2) = axis_coeff; - break; - case Z_AXIS: - pose_coeffs(0) = axis_coeff; - pose_coeffs(1) = axis_coeff; - break; - } + VectorOfVectorPtr f_ineq(new AlignedAxisErrCalculator(input_q.matrix(), prob.GetKin(), prob.GetEnv(), + link, axis, tolerance, tcp_q.matrix())); + + Eigen::Vector4d coeffs(axis_coeff, axis_coeff, axis_coeff, angle_coeff); // add the costs or constraints depending on the term type if (term_type == TT_COST) { - prob.addCost(CostPtr(new CostFromErrFunc(f_eq, prob.GetVarRow(timestep), pose_coeffs, ABS, name))); - prob.addCost(CostPtr(new CostFromErrFunc(f_ineq, prob.GetVarRow(timestep), ineq_coeffs, HINGE, name))); + prob.addCost(CostPtr(new CostFromErrFunc(f_ineq, prob.GetVarRow(timestep), coeffs, HINGE, name))); } else if (term_type == TT_CNT) { - prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_eq, prob.GetVarRow(timestep), pose_coeffs, EQ, name))); - prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_ineq, prob.GetVarRow(timestep), ineq_coeffs, INEQ, name))); + prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_ineq, prob.GetVarRow(timestep), coeffs, INEQ, name))); } } // initialize with basic default values -ConicalAxisTermInfo::ConicalAxisTermInfo() { - pos_coeffs = Vector3d::Ones(); - axis_coeff = 1.0; - conical_coeff = 1.0; - tcp.setIdentity(); +ConicalAxisTermInfo::ConicalAxisTermInfo() : tcp_wxyz(1.0, 0.0, 0.0, 0.0) { + weight = 0.0; } // construct the term from a Json file @@ -937,32 +870,15 @@ void ConicalAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value &v) // make sure params is a member of the Json value provided FAIL_IF_FALSE(v.isMember("params")); - // provide default position and orienation for the tcp (tool center point) - Vector3d tcp_xyz = Vector3d::Zero(); - Vector4d tcp_wxyz = Vector4d(1, 0, 0, 0); - // get the params value and read the parameters fmro the file into the object members const Value& params = v["params"]; childFromJson(params, timestep, "timestep", pci.basic_info.n_steps-1); - childFromJson(params, xyz,"xyz"); - childFromJson(params, wxyz,"wxyz"); - childFromJson(params, pos_coeffs, "pos_coeffs", (Vector3d)Vector3d::Ones()); - childFromJson(params, axis_coeff, "axis_coeff", (double)1.0); - childFromJson(params, conical_coeff, "conical_coeff", (double)1.0); - childFromJson(params, link, "link"); - - string axis_str; - childFromJson(params, axis_str, "axis"); - axis = stringToAxisEnum(axis_str); - - childFromJson(params, tol, "tolerance"); - - // construct the tcp Affine matrix from its components - childFromJson(params, tcp_xyz, "tcp,xyz", tcp_xyz); + childFromJson(params, wxyz, "wxyz"); childFromJson(params, tcp_wxyz, "tcp_xwyz", tcp_wxyz); - Eigen::Quaterniond q(tcp_wxyz(0), tcp_wxyz(1), tcp_wxyz(2), tcp_wxyz(3)); - tcp.linear() = q.matrix(); - tcp.translation() = tcp_xyz; + childFromJson(params, weight, "weight"); + childFromJson(params, link, "link"); + childFromJson(params, axis, "axis"); + childFromJson(params, tolerance, "tolerance"); // make sure the link name provided is valid const std::vector& link_names = pci.kin->getLinkNames(); @@ -971,52 +887,30 @@ void ConicalAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value &v) } // make sure there are no extra items in the params value - const char* all_fields[] = {"timestep", "xyz", "wxyz", "pos_coeffs", "axis_coeff", "conical_coeff", "link", - "tolerance", "axis", "tcp_xyz", "tcp_wxyz"}; + const char* all_fields[] = {"timestep", "wxyz", "weight", "link", + "tolerance", "axis", "tcp_wxyz"}; ensure_only_members(params, all_fields, sizeof(all_fields)/sizeof(char*)); } // add the term to the problem description void ConicalAxisTermInfo::hatch(TrajOptProb &prob) { // construct the desired pose - Eigen::Affine3d input_pose; - Eigen::Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); - input_pose.linear() = q.matrix(); - input_pose.translation() = xyz; + Eigen::Quaterniond input_q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + Eigen::Quaterniond tcp_q(tcp_wxyz(0), tcp_wxyz(1), tcp_wxyz(2), tcp_wxyz(3)); // create the equality and inequality error functions to be used - VectorOfVectorPtr f_eq(new StaticCartPoseErrCalculator(input_pose, prob.GetKin(), prob.GetEnv(), link, tcp)); - VectorOfVectorPtr f_ineq(new ConicalAxisErrCalculator(input_pose, prob.GetKin(), prob.GetEnv(), link, axis, tol, tcp)); - - // construct the coefficient vectors for the errors - VectorXd pose_coeffs = concat(Vector3d(0.0, 0.0, 0.0), pos_coeffs); - VectorXd ineq_coeffs(1); - - // the inequality coefficient is the one referrring to the error outside the conical tolerance - ineq_coeffs(0) = conical_coeff; - - // the error corresponding to the rotation about the axis in the cone is given by - // the quaternion component of that axis - switch(axis) { - case X_AXIS: - pose_coeffs(0) = axis_coeff; - break; - case Y_AXIS: - pose_coeffs(1) = axis_coeff; - break; - case Z_AXIS: - pose_coeffs(2) = axis_coeff; - break; - } + VectorOfVectorPtr f_ineq(new ConicalAxisErrCalculator(input_q.matrix(), prob.GetKin(), prob.GetEnv(), + link, axis, tolerance, tcp_q.matrix())); + + VectorXd coeffs(1); + coeffs(0) = weight; // add the costs or constraints depending on the term type if (term_type == TT_COST) { - prob.addCost(CostPtr(new CostFromErrFunc(f_eq, prob.GetVarRow(timestep), pose_coeffs, ABS, name))); - prob.addCost(CostPtr(new CostFromErrFunc(f_ineq, prob.GetVarRow(timestep), ineq_coeffs, HINGE, name))); + prob.addCost(CostPtr(new CostFromErrFunc(f_ineq, prob.GetVarRow(timestep), coeffs, HINGE, name))); } else if (term_type == TT_CNT) { - prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_eq, prob.GetVarRow(timestep), pose_coeffs, EQ, name))); - prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_ineq, prob.GetVarRow(timestep), ineq_coeffs, INEQ, name))); + prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_ineq, prob.GetVarRow(timestep), coeffs, INEQ, name))); } } diff --git a/trajopt/test/angular_constraint_unit.cpp b/trajopt/test/angular_constraint_unit.cpp index 426ce934..724e0910 100644 --- a/trajopt/test/angular_constraint_unit.cpp +++ b/trajopt/test/angular_constraint_unit.cpp @@ -20,12 +20,14 @@ using namespace trajopt; using namespace tesseract; +using namespace Eigen; const std::string ROBOT_DESCRIPTION_PARAM = "robot_description"; /**< Default ROS parameter for robot description */ const std::string ROBOT_SEMANTIC_PARAM = "robot_description_semantic"; /**< Default ROS parameter for robot description */ bool plotting_ = false; int steps_ = 5; +double approx_equals_diff_ = 1e-2; struct testInfo { std::string method; @@ -62,28 +64,20 @@ class AngularConstraintTest : public testing::TestWithParam { ProblemConstructionInfo pci(env_); pci.fromJson(root); - for (int i = 0; i < steps_; i++) { + double y[] = {-0.2, -0.1, 0.0, 0.1, 0.2}; + for (int i = 0; i < 5; i++) + { + Vector3d xyz(0.5, y[i], 0.62); + Vector4d wxyz(0.0, 0.0, 1.0, 0.0); + Affine3d cur_pose; - if (constraint_type_ == "confined") { - std::shared_ptr pose = static_pointer_cast(pci.cnt_infos.at(i)); - cur_pose.translation() = pose->xyz; - Quaterniond q(pose->wxyz(0), pose->wxyz(1), pose->wxyz(2), pose->wxyz(3)); - cur_pose.linear() = q.matrix(); - pose_inverses_.insert(pose_inverses_.begin() + i, cur_pose.inverse()); - } - else if (constraint_type_ == "conical") { - std::shared_ptr pose = static_pointer_cast(pci.cnt_infos.at(i)); - cur_pose.translation() = pose->xyz; - Quaterniond q(pose->wxyz(0), pose->wxyz(1), pose->wxyz(2), pose->wxyz(3)); - cur_pose.linear() = q.matrix(); - pose_inverses_.insert(pose_inverses_.begin() + i, cur_pose.inverse()); - } - else { - ROS_ERROR("%s is not a valid constraint type. Exiting", constraint_type_.c_str()); - exit(-1); - } + cur_pose.translation() = xyz; + Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); + cur_pose.linear() = q.matrix(); + pose_inverses_.insert(pose_inverses_.begin() + i, cur_pose.inverse()); } + return ConstructProblem(pci); } @@ -136,35 +130,42 @@ class AngularConstraintTest : public testing::TestWithParam { cur_pose.linear() = q.matrix(); pose_inverses_.insert(pose_inverses_.begin() + i, cur_pose.inverse()); - if (constraint_type_ == "confined") { - std::shared_ptr pose(new ConfinedAxisTermInfo); - pose->tol = tol_; - pose->axis = 'y'; - pose->term_type = TT_CNT; - pose->name = "waypoint_cart_" + std::to_string(i); - pose->link = "tool0"; - pose->timestep = i; - pose->xyz = Eigen::Vector3d(0.5, -0.2 + delta * i, 0.62); - pose->wxyz = Eigen::Vector4d(0.0, 0.0, 1.0, 0.0); - pose->pos_coeffs = Eigen::Vector3d(10, 10, 10); - pose->axis_coeff = 10.0; - pose->confined_coeff = 10.0; - pci.cnt_infos.push_back(pose); + std::shared_ptr position(new StaticPoseCostInfo); + position->name = "position_" + std::to_string(i); + position->term_type = TT_CNT; + position->name = "waypoint_cart_" + std::to_string(i); + position->link = "tool0"; + position->timestep = i; + position->xyz = xyz; + position->wxyz = wxyz; + position->pos_coeffs = Eigen::Vector3d(10, 10, 10); + position->rot_coeffs = Eigen::Vector3d::Zero(); + pci.cnt_infos.push_back(position); + + if (constraint_type_ == "aligned") { + std::shared_ptr aligned(new AlignedAxisTermInfo); + aligned->tolerance = tol_; + aligned->axis = Vector3d::UnitX(); + aligned->term_type = TT_CNT; + aligned->name = "aligned_" + std::to_string(i); + aligned->link = "tool0"; + aligned->timestep = i; + aligned->wxyz = Eigen::Vector4d(0.0, 0.0, 1.0, 0.0); + aligned->axis_coeff = 2.0; + aligned->angle_coeff = 10.0; + pci.cnt_infos.push_back(aligned); } else if (constraint_type_ == "conical") { - std::shared_ptr pose(new ConicalAxisTermInfo); - pose->tol = tol_; - pose->axis = 'z'; - pose->term_type = TT_CNT; - pose->name = "waypoint_cart_" + std::to_string(i); - pose->link = "tool0"; - pose->timestep = i; - pose->xyz = Eigen::Vector3d(0.5, -0.2 + delta * i, 0.62); - pose->wxyz = Eigen::Vector4d(0.0, 0.0, 1.0, 0.0); - pose->pos_coeffs = Eigen::Vector3d(10, 10, 10); - pose->axis_coeff = 10.0; - pose->conical_coeff = 10.0; - pci.cnt_infos.push_back(pose); + std::shared_ptr conical(new ConicalAxisTermInfo); + conical->tolerance = tol_; + conical->axis = Vector3d::UnitZ(); + conical->term_type = TT_CNT; + conical->name = "conical_" + std::to_string(i); + conical->link = "tool0"; + conical->timestep = i; + conical->wxyz = Eigen::Vector4d(0.0, 0.0, 1.0, 0.0); + conical->weight = 10.0; + pci.cnt_infos.push_back(conical); } else { ROS_ERROR("%s is not a valid constraint type. Exiting", constraint_type_.c_str()); @@ -258,9 +259,9 @@ TEST_P(AngularConstraintTest, AngularConstraint) std::string method = info.method; constraint_type_ = info.constraint; - int num_iterations = method == "cpp" ? 5 : 1; + int num_iterations = method == "cpp" ? 3 : 1; for (int i = 0; i < num_iterations; i++) { - tol_ = method == "cpp" ? 2.5 * (i + 1) : 15.0; + tol_ = (method == "cpp" ? 5.0 * (i + 1) : 5.0) * M_PI/180.0;; if (method == "cpp") { prob = cppMethod(); @@ -322,42 +323,28 @@ TEST_P(AngularConstraintTest, AngularConstraint) Affine3d pose_inv = pose_inverses_.at(j); Affine3d err = pose_inv * (pose * tcp); - if (constraint_type_ == "confined") { + if (constraint_type_ == "aligned") { AngleAxisd aa(err.rotation()); - EXPECT_LE(aa.angle(), tol_ * M_PI/180.0 * (1.0 + 1e-4) ); + double aligned_angle = aa.angle(); + EXPECT_LE(aligned_angle, tol_ * (1.0 + approx_equals_diff_) ); Vector3d axis = aa.axis(); - double axis_x = axis(0); - double axis_y = axis(1); - double axis_z = axis(2); - EXPECT_NEAR(0.0, axis_x, 1e-2); - if (axis_y > 0) { - EXPECT_NEAR(1.0, axis_y, 1e-4); - } - else { - EXPECT_NEAR(-1.0, axis_y, 1e-2); - } - EXPECT_NEAR(0.0, axis_z, 1e-3); - + double dot_prod = axis.dot(Vector3d::UnitX()); + EXPECT_NEAR(1.0, fabs(dot_prod), approx_equals_diff_); } - else { + else { // conical Matrix3d orientation(err.rotation()); - double angle = acos(orientation(2,2)) * 180.0/M_PI; - EXPECT_LE(angle, tol_ * (1 + 1e-4)); + double conical_angle = acos(orientation(2, 2)); + EXPECT_LE(conical_angle, tol_ * (1 + approx_equals_diff_)); } - - Vector3d pos = err.translation(); - EXPECT_NEAR(0.0, pos(0), 1e-4); - EXPECT_NEAR(0.0, pos(1), 1e-4); - EXPECT_NEAR(0.0, pos(2), 1e-4); } } } -INSTANTIATE_TEST_CASE_P(Tests, AngularConstraintTest, ::testing::Values(testInfo("json", "confined"), testInfo("json", "conical"), - testInfo("cpp", "confined"), testInfo("cpp", "conical")) ); +INSTANTIATE_TEST_CASE_P(Tests, AngularConstraintTest, ::testing::Values(testInfo("json", "aligned"), + testInfo("json", "conical"), testInfo("cpp", "aligned"), testInfo("cpp", "conical")) ); int main(int argc, char** argv) { diff --git a/trajopt/test/angular_constraint_unit.launch b/trajopt/test/angular_constraint_unit.launch index 0d98edd5..72e2bc0f 100644 --- a/trajopt/test/angular_constraint_unit.launch +++ b/trajopt/test/angular_constraint_unit.launch @@ -7,7 +7,7 @@ - + @@ -29,7 +29,7 @@ - + diff --git a/trajopt/test/basic_cartesian_plan_aligned_axis.json b/trajopt/test/basic_cartesian_plan_aligned_axis.json new file mode 100644 index 00000000..ab122aab --- /dev/null +++ b/trajopt/test/basic_cartesian_plan_aligned_axis.json @@ -0,0 +1,169 @@ +{ + "basic_info" : + { + "n_steps" : 5, + "manip" : "manipulator", + "start_fixed" : false + }, + "costs" : + [ + { + "type" : "joint_vel", + "params": + { + "coeffs" : [5] + } + }, + { + "type" : "collision", + "params" : + { + "coeffs" : [20], + "dist_pen" : [0.025], + "continuous" : false + } + } + ], + "constraints" : + [ + { + "name" : "aligned_1", + "type" : "aligned_axis", + "params" : + { + "timestep" : 0, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "axis_coeff" : 2.0, + "angle_coeff" : 10.0, + "axis" : [1, 0, 0], + "tolerance" : 0.087266 + } + }, + { + "name" : "aligned_2", + "type" : "aligned_axis", + "params" : + { + "timestep" : 1, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "axis_coeff" : 2.0, + "angle_coeff" : 10.0, + "axis" : [1, 0, 0], + "tolerance" : 0.087266 + } + }, + { + "name" : "aligned_3", + "type" : "aligned_axis", + "params" : + { + "timestep" : 2, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "axis_coeff" : 2.0, + "angle_coeff" : 10.0, + "axis" : [1, 0, 0], + "tolerance" : 0.087266 + } + }, + { + "name" : "aligned_4", + "type" : "aligned_axis", + "params" : + { + "timestep" : 3, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "axis_coeff" : 2.0, + "angle_coeff" : 10.0, + "axis" : [1, 0, 0], + "tolerance" : 0.087266 + } + }, + { + "name" : "aligned_5", + "type" : "aligned_axis", + "params" : + { + "timestep" : 4, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "axis_coeff" : 2.0, + "angle_coeff" : 10.0, + "axis" : [1, 0, 0], + "tolerance" : 0.087266 + } + }, + { + "name" : "waypoint_cart_1", + "type" : "static_pose", + "params" : + { + "timestep" : 0, + "xyz" : [0.5, -0.2, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_2", + "type" : "static_pose", + "params" : + { + "timestep" : 1, + "xyz" : [0.5, -0.1, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_3", + "type" : "static_pose", + "params" : + { + "timestep" : 2, + "xyz" : [0.5, 0.0, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_4", + "type" : "static_pose", + "params" : + { + "timestep" : 3, + "xyz" : [0.5, 0.1, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + }, + { + "name" : "waypoint_cart_5", + "type" : "static_pose", + "params" : + { + "timestep" : 4, + "xyz" : [0.5, 0.2, 0.62], + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "pos_coeffs" : [10, 10, 10], + "rot_coeffs" : [0, 0, 0] + } + } + ], + "init_info" : + { + "type" : "stationary" + } +} diff --git a/trajopt/test/basic_cartesian_plan_confined_axis.json b/trajopt/test/basic_cartesian_plan_confined_axis.json deleted file mode 100644 index 594ab9ce..00000000 --- a/trajopt/test/basic_cartesian_plan_confined_axis.json +++ /dev/null @@ -1,114 +0,0 @@ -{ - "basic_info" : - { - "n_steps" : 5, - "manip" : "manipulator", - "start_fixed" : false - }, - "costs" : - [ - { - "type" : "joint_vel", - "params": - { - "coeffs" : [5] - } - }, - { - "type" : "collision", - "params" : - { - "coeffs" : [20], - "dist_pen" : [0.025], - "continuous" : false - } - } - ], - "constraints" : - [ - { - "name" : "waypoint_cart_1", - "type" : "confined_axis", - "params" : - { - "timestep" : 0, - "xyz" : [0.5, -0.2, 0.62], - "wxyz" : [0.0, 0.0, 1.0, 0.0], - "link" : "tool0", - "pos_coeffs" : [10, 10, 10], - "axis_coeff" : 10.0, - "confined_coeff" : 10.0, - "axis" : "Y_AXIS", - "tolerance" : 15.0 - } - }, - { - "name" : "waypoint_cart_2", - "type" : "confined_axis", - "params" : - { - "timestep" : 1, - "xyz" : [0.5, -0.1, 0.62], - "wxyz" : [0.0, 0.0, 1.0, 0.0], - "link" : "tool0", - "pos_coeffs" : [10, 10, 10], - "axis_coeff" : 10.0, - "confined_coeff" : 10.0, - "axis" : "Y_AXIS", - "tolerance" : 15.0 - } - }, - { - "name" : "waypoint_cart_3", - "type" : "confined_axis", - "params" : - { - "timestep" : 2, - "xyz" : [0.5, 0.0, 0.62], - "wxyz" : [0.0, 0.0, 1.0, 0.0], - "link" : "tool0", - "pos_coeffs" : [10, 10, 10], - "axis_coeff" : 10.0, - "confined_coeff" : 10.0, - "axis" : "Y_AXIS", - "tolerance" : 15.0 - } - }, - { - "name" : "waypoint_cart_4", - "type" : "confined_axis", - "params" : - { - "timestep" : 3, - "xyz" : [0.5, 0.1, 0.62], - "wxyz" : [0.0, 0.0, 1.0, 0.0], - "link" : "tool0", - "pos_coeffs" : [10, 10, 10], - "axis_coeff" : 10.0, - "confined_coeff" : 10.0, - "axis" : "Y_AXIS", - "tolerance" : 15.0 - } - }, - { - "name" : "waypoint_cart_5", - "type" : "confined_axis", - "params" : - { - "timestep" : 4, - "xyz" : [0.5, 0.2, 0.62], - "wxyz" : [0.0, 0.0, 1.0, 0.0], - "link" : "tool0", - "pos_coeffs" : [10, 10, 10], - "axis_coeff" : 10.0, - "confined_coeff" : 10.0, - "axis" : "Y_AXIS", - "tolerance" : 15.0 - } - } - ], - "init_info" : - { - "type" : "stationary" - } -} diff --git a/trajopt/test/basic_cartesian_plan_conical_axis.json b/trajopt/test/basic_cartesian_plan_conical_axis.json index bc0ebbfc..18ad2c63 100644 --- a/trajopt/test/basic_cartesian_plan_conical_axis.json +++ b/trajopt/test/basic_cartesian_plan_conical_axis.json @@ -27,24 +27,86 @@ "constraints" : [ { - "name" : "waypoint_cart_1", + "name" : "conical_1", + "type" : "conical_axis", + "params" : + { + "timestep" : 0, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "weight" : 10.0, + "axis" : [0, 0, 1], + "tolerance" : 0.087266 + } + }, + { + "name" : "conical_2", + "type" : "conical_axis", + "params" : + { + "timestep" : 1, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "weight" : 10.0, + "axis" : [0, 0, 1], + "tolerance" : 0.087266 + } + }, + { + "name" : "conical_3", + "type" : "conical_axis", + "params" : + { + "timestep" : 2, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "weight" : 10.0, + "axis" : [0, 0, 1], + "tolerance" : 0.087266 + } + }, + { + "name" : "conical_4", "type" : "conical_axis", "params" : + { + "timestep" : 3, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "weight" : 10.0, + "axis" : [0, 0, 1], + "tolerance" : 0.087266 + } + }, + { + "name" : "conical_5", + "type" : "conical_axis", + "params" : + { + "timestep" : 4, + "wxyz" : [0.0, 0.0, 1.0, 0.0], + "link" : "tool0", + "weight" : 10.0, + "axis" : [0, 0, 1], + "tolerance" : 0.087266 + } + }, + { + "name" : "waypoint_cart_1", + "type" : "static_pose", + "params" : { "timestep" : 0, "xyz" : [0.5, -0.2, 0.62], "wxyz" : [0.0, 0.0, 1.0, 0.0], "link" : "tool0", "pos_coeffs" : [10, 10, 10], - "axis_coeff" : 10.0, - "conical_coeff" : 10.0, - "axis" : "Z_AXIS", - "tolerance" : 15.0 + "rot_coeffs" : [0, 0, 0] } }, { "name" : "waypoint_cart_2", - "type" : "conical_axis", + "type" : "static_pose", "params" : { "timestep" : 1, @@ -52,15 +114,12 @@ "wxyz" : [0.0, 0.0, 1.0, 0.0], "link" : "tool0", "pos_coeffs" : [10, 10, 10], - "axis_coeff" : 10.0, - "conical_coeff" : 10.0, - "axis" : "Z_AXIS", - "tolerance" : 15.0 + "rot_coeffs" : [0, 0, 0] } }, { "name" : "waypoint_cart_3", - "type" : "conical_axis", + "type" : "static_pose", "params" : { "timestep" : 2, @@ -68,15 +127,12 @@ "wxyz" : [0.0, 0.0, 1.0, 0.0], "link" : "tool0", "pos_coeffs" : [10, 10, 10], - "axis_coeff" : 10.0, - "conical_coeff" : 10.0, - "axis" : "Z_AXIS", - "tolerance" : 15.0 + "rot_coeffs" : [0, 0, 0] } }, { "name" : "waypoint_cart_4", - "type" : "conical_axis", + "type" : "static_pose", "params" : { "timestep" : 3, @@ -84,15 +140,12 @@ "wxyz" : [0.0, 0.0, 1.0, 0.0], "link" : "tool0", "pos_coeffs" : [10, 10, 10], - "axis_coeff" : 10.0, - "conical_coeff" : 10.0, - "axis" : "Z_AXIS", - "tolerance" : 15.0 + "rot_coeffs" : [0, 0, 0] } }, { "name" : "waypoint_cart_5", - "type" : "conical_axis", + "type" : "static_pose", "params" : { "timestep" : 4, @@ -100,10 +153,7 @@ "wxyz" : [0.0, 0.0, 1.0, 0.0], "link" : "tool0", "pos_coeffs" : [10, 10, 10], - "axis_coeff" : 10.0, - "conical_coeff" : 10.0, - "axis" : "Z_AXIS", - "tolerance" : 15.0 + "rot_coeffs" : [0, 0, 0] } } ], From d023a4188535eec2502a8c6597c65d24804419ad Mon Sep 17 00:00:00 2001 From: Reid Christopher Date: Fri, 24 Aug 2018 15:58:37 -0500 Subject: [PATCH 4/4] Clang format --- trajopt/include/trajopt/kinematic_terms.hpp | 75 +++++++++-------- .../include/trajopt/problem_description.hpp | 39 +++++---- trajopt/src/kinematic_terms.cpp | 17 ++-- trajopt/src/problem_description.cpp | 68 ++++++++------- trajopt/test/angular_constraint_unit.cpp | 83 ++++++++++--------- 5 files changed, 154 insertions(+), 128 deletions(-) diff --git a/trajopt/include/trajopt/kinematic_terms.hpp b/trajopt/include/trajopt/kinematic_terms.hpp index dc1d0d75..eaf4bf73 100644 --- a/trajopt/include/trajopt/kinematic_terms.hpp +++ b/trajopt/include/trajopt/kinematic_terms.hpp @@ -108,16 +108,16 @@ struct CartVelCalculator : VectorOfVector * @brief The AlignedAxisErrCalculator is a struct whose operator() calculates error of a given * pose with respect to the rotation along the defined axis. */ -struct AlignedAxisErrCalculator : public VectorOfVector { - - Eigen::Matrix3d orientation_inv_; /**< @brief param The inverse of the desired orientation */ +struct AlignedAxisErrCalculator : public VectorOfVector +{ + Eigen::Matrix3d orientation_inv_; /**< @brief param The inverse of the desired orientation */ tesseract::BasicKinConstPtr manip_; /**< @brief Kinematics object */ - tesseract::BasicEnvConstPtr env_; /**< @brief Environment object */ - std::string link_; /**< @brief Link of the robot referred to */ - Eigen::Matrix3d tcp_orientation_; /**< @brief Tool center point orientation */ + tesseract::BasicEnvConstPtr env_; /**< @brief Environment object */ + std::string link_; /**< @brief Link of the robot referred to */ + Eigen::Matrix3d tcp_orientation_; /**< @brief Tool center point orientation */ Vector3d axis_; /**< @brief Axis of rotation to align with */ - double tol_; /**< @brief Tolerance angle in radians */ + double tol_; /**< @brief Tolerance angle in radians */ /** * @brief AlignedAxisErrCalculator Constructor @@ -129,15 +129,20 @@ struct AlignedAxisErrCalculator : public VectorOfVector { * @param tol_angle Tolerance in radians * @param tcp Tool center point */ - AlignedAxisErrCalculator(const Eigen::Matrix3d& orientation, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env, - std::string link, Vector3d axis, double tol, Eigen::Matrix3d tcp_orientation = Eigen::Matrix3d::Identity()) : - orientation_inv_(orientation.inverse()), - manip_(manip), - env_(env), - link_(link), - tcp_orientation_(tcp_orientation), - axis_(axis), - tol_(tol) + AlignedAxisErrCalculator(const Eigen::Matrix3d& orientation, + tesseract::BasicKinConstPtr manip, + tesseract::BasicEnvConstPtr env, + std::string link, + Vector3d axis, + double tol, + Eigen::Matrix3d tcp_orientation = Eigen::Matrix3d::Identity()) + : orientation_inv_(orientation.inverse()) + , manip_(manip) + , env_(env) + , link_(link) + , tcp_orientation_(tcp_orientation) + , axis_(axis) + , tol_(tol) { } @@ -147,7 +152,6 @@ struct AlignedAxisErrCalculator : public VectorOfVector { * @return 1D vector of error beyond the allowed rotation */ VectorXd operator()(const VectorXd& dof_vals) const; - }; /** @@ -155,15 +159,16 @@ struct AlignedAxisErrCalculator : public VectorOfVector { * given pose with respect to the conical constraint defined * */ -struct ConicalAxisErrCalculator : public VectorOfVector { - Eigen::Matrix3d orientation_inv_; /**< @brief Inverse of the desired orientation */ +struct ConicalAxisErrCalculator : public VectorOfVector +{ + Eigen::Matrix3d orientation_inv_; /**< @brief Inverse of the desired orientation */ tesseract::BasicKinConstPtr manip_; /**< @brief Kinematics object */ - tesseract::BasicEnvConstPtr env_; /**< @brief Environment object */ - std::string link_; /**< @brief The link of the robot referred to */ - Eigen::Matrix3d tcp_orientation_; /**< @brief Tool center point orientation */ + tesseract::BasicEnvConstPtr env_; /**< @brief Environment object */ + std::string link_; /**< @brief The link of the robot referred to */ + Eigen::Matrix3d tcp_orientation_; /**< @brief Tool center point orientation */ Vector3d axis_; /**< @brief Axis the conical tolerance is applied to */ - double tol_; /**< @brief Tolerance angle in radians */ + double tol_; /**< @brief Tolerance angle in radians */ /** * @brief ConicalAxisErrCalculator @@ -175,15 +180,20 @@ struct ConicalAxisErrCalculator : public VectorOfVector { * @param tol_angle Tolerance angle in degrees * @param tcp Tool center point */ - ConicalAxisErrCalculator(const Eigen::Matrix3d& orientation, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env, - std::string link, Vector3d axis, double tol, Eigen::Matrix3d tcp_orientation = Eigen::Matrix3d::Identity()) : - orientation_inv_(orientation.inverse()), - manip_(manip), - env_(env), - link_(link), - tcp_orientation_(tcp_orientation), - axis_(axis), - tol_(tol) + ConicalAxisErrCalculator(const Eigen::Matrix3d& orientation, + tesseract::BasicKinConstPtr manip, + tesseract::BasicEnvConstPtr env, + std::string link, + Vector3d axis, + double tol, + Eigen::Matrix3d tcp_orientation = Eigen::Matrix3d::Identity()) + : orientation_inv_(orientation.inverse()) + , manip_(manip) + , env_(env) + , link_(link) + , tcp_orientation_(tcp_orientation) + , axis_(axis) + , tol_(tol) { } @@ -194,5 +204,4 @@ struct ConicalAxisErrCalculator : public VectorOfVector { */ VectorXd operator()(const VectorXd& dof_vals) const; }; - } diff --git a/trajopt/include/trajopt/problem_description.hpp b/trajopt/include/trajopt/problem_description.hpp index 9cca6e87..22e44ac4 100644 --- a/trajopt/include/trajopt/problem_description.hpp +++ b/trajopt/include/trajopt/problem_description.hpp @@ -13,7 +13,6 @@ struct OptResults; namespace trajopt { - using namespace json_marshal; using namespace Json; @@ -329,22 +328,25 @@ struct JointConstraintInfo : public TermInfo, public MakesConstraint * @brief The AlignedAxisTermInfo struct contains information to create constraints or costs * using a tolerance for the rotation about an axis */ -struct AlignedAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost { +struct AlignedAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost +{ int timestep; /**< @brief The timestep of this term in the trajectory */ - Vector4d wxyz; /**< @brief 4D vector containing w, x, y, and z quaternion components for the pose orientation (in that order) */ - Vector4d tcp_wxyz; /**< @brief Tool center point */ - double axis_coeff; /**< @brief Coefficient multiplied by the errors of the rotation axis from the specified axis */ + Vector4d + wxyz; /**< @brief 4D vector containing w, x, y, and z quaternion components for the pose orientation (in that + order) */ + Vector4d tcp_wxyz; /**< @brief Tool center point */ + double axis_coeff; /**< @brief Coefficient multiplied by the errors of the rotation axis from the specified axis */ double angle_coeff; /**< @brief Coefficient multipleid by the angle of rotation past the tolerance */ - string link; /**< @brief Link of the robot the term refers to */ - Vector3d axis; /**< @brief Axis allowed to rotate with respect to the frame of #link */ - double tolerance; /**< @brief Rotation tolerance about the given axis in radians */ + string link; /**< @brief Link of the robot the term refers to */ + Vector3d axis; /**< @brief Axis allowed to rotate with respect to the frame of #link */ + double tolerance; /**< @brief Rotation tolerance about the given axis in radians */ AlignedAxisTermInfo(); /** * @brief fromJson Constructs the term from a Json file */ - void fromJson(ProblemConstructionInfo &pci, const Value& v); + void fromJson(ProblemConstructionInfo& pci, const Value& v); /** * @brief hatch Add the proper cost/constraint functions to the problem @@ -358,22 +360,24 @@ struct AlignedAxisTermInfo : public TermInfo, public MakesConstraint, public Mak * @brief The ConicalAxisTermInfo struct contains information to create constraints or costs * using a conical tolerance about an axis */ -struct ConicalAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost { +struct ConicalAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost +{ int timestep; /**< @brief The timestep of this term in the trajectory */ - Vector4d wxyz; /**< @brief 4D vector containing w, x, y, and z quaternion components for the pose orientation (in that order) */ + Vector4d + wxyz; /**< @brief 4D vector containing w, x, y, and z quaternion components for the pose orientation (in that + order) */ Vector4d tcp_wxyz; /**< @brief Tool center point */ - double weight; /**< @brief Coefficient multiplied by the errors of the rotation axis from the specified axis */ - string link; /**< @brief Link of the robot the term refers to */ - Vector3d axis; /**< @brief Axis around which the conical tolerance will be applied */ - double tolerance; /**< @brief Tolerance angle of the cone in radians*/ - + double weight; /**< @brief Coefficient multiplied by the errors of the rotation axis from the specified axis */ + string link; /**< @brief Link of the robot the term refers to */ + Vector3d axis; /**< @brief Axis around which the conical tolerance will be applied */ + double tolerance; /**< @brief Tolerance angle of the cone in radians*/ ConicalAxisTermInfo(); /** * @brief fromJson Constructs the term from a Json file */ - void fromJson(ProblemConstructionInfo &pci, const Value& v); + void fromJson(ProblemConstructionInfo& pci, const Value& v); /** * @brief hatch Add the proper cost/constraint functions to the problem @@ -382,5 +386,4 @@ struct ConicalAxisTermInfo : public TermInfo, public MakesConstraint, public Mak void hatch(TrajOptProb& prob); DEFINE_CREATE(ConicalAxisTermInfo) }; - } diff --git a/trajopt/src/kinematic_terms.cpp b/trajopt/src/kinematic_terms.cpp index 991bbe54..8c246163 100644 --- a/trajopt/src/kinematic_terms.cpp +++ b/trajopt/src/kinematic_terms.cpp @@ -176,7 +176,8 @@ VectorXd CartVelCalculator::operator()(const VectorXd& dof_vals) const return out; } -VectorXd AlignedAxisErrCalculator::operator()(const VectorXd& dof_vals) const { +VectorXd AlignedAxisErrCalculator::operator()(const VectorXd& dof_vals) const +{ // calculate the current pose given the DOF values for the robot Affine3d new_pose, change_base; change_base = env_->getLinkTransform(manip_->getBaseLinkName()); @@ -192,16 +193,17 @@ VectorXd AlignedAxisErrCalculator::operator()(const VectorXd& dof_vals) const { // the angle error is typical: actual value - tolerance double angle_err = angle - tol_; - // the axis error is scaled with the angle. This is for two reasons: - // 1. The error terms will stay on the same scale, since 1 - |dot_prod| ranges from 0 to 1 - // 2. As the angle approaches zero, the axis loses meaning and should not be counted as erroneous. - Vector3d axis_err = (axis_ - (orientation_err*axis_)).array().square(); + // axis error + // the element wise squared difference between the desired axis and + // the desired axis after being rotated by the current pose "error" + Vector3d axis_err = (axis_ - (orientation_err * axis_)).array().square(); Vector4d err(axis_err.x(), axis_err.y(), axis_err.z(), angle_err); return err; } -VectorXd ConicalAxisErrCalculator::operator()(const VectorXd& dof_vals) const { +VectorXd ConicalAxisErrCalculator::operator()(const VectorXd& dof_vals) const +{ // calculate the current pose given the DOF values for the robot Affine3d new_pose, change_base; change_base = env_->getLinkTransform(manip_->getBaseLinkName()); @@ -212,9 +214,8 @@ VectorXd ConicalAxisErrCalculator::operator()(const VectorXd& dof_vals) const { VectorXd err(1); // determine the error of the conical axis - err(0) = acos((orientation_err*axis_).dot(axis_)) - tol_; + err(0) = acos((orientation_err * axis_).dot(axis_)) - tol_; return err; } - } diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp index c5289974..db867227 100644 --- a/trajopt/src/problem_description.cpp +++ b/trajopt/src/problem_description.cpp @@ -68,7 +68,6 @@ BoolVec toMask(const VectorXd& x) { return out; } #endif - } namespace trajopt @@ -806,19 +805,21 @@ void JointConstraintInfo::hatch(TrajOptProb& prob) } // initialize with basic default values -AlignedAxisTermInfo::AlignedAxisTermInfo() : tcp_wxyz(1.0, 0.0, 0.0, 0.0) { +AlignedAxisTermInfo::AlignedAxisTermInfo() : tcp_wxyz(1.0, 0.0, 0.0, 0.0) +{ axis_coeff = 0.0; angle_coeff = 0.0; } // get the term information from a json value -void AlignedAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value& v) { +void AlignedAxisTermInfo::fromJson(ProblemConstructionInfo& pci, const Value& v) +{ // make sure params is a member of the Json value provided FAIL_IF_FALSE(v.isMember("params")); // get the params value and read the parameters fmro the file into the object members const Value& params = v["params"]; - childFromJson(params, timestep, "timestep", pci.basic_info.n_steps-1); + childFromJson(params, timestep, "timestep", pci.basic_info.n_steps - 1); childFromJson(params, wxyz, "wxyz"); childFromJson(params, tcp_wxyz, "tcp_xwyz", tcp_wxyz); childFromJson(params, axis_coeff, "axis_coeff"); @@ -829,50 +830,52 @@ void AlignedAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value& v) // make sure the link name provided is valid const std::vector& link_names = pci.kin->getLinkNames(); - if (std::find(link_names.begin(), link_names.end(),link)==link_names.end()) { - PRINT_AND_THROW(boost::format("invalid link name: %s")%link); + if (std::find(link_names.begin(), link_names.end(), link) == link_names.end()) + { + PRINT_AND_THROW(boost::format("invalid link name: %s") % link); } // make sure there are no extra items in the params value - const char* all_fields[] = {"timestep", "wxyz", "axis_coeff", "angle_coeff", "link", - "tolerance", "axis", "tcp_wxyz"}; - ensure_only_members(params, all_fields, sizeof(all_fields)/sizeof(char*)); + const char* all_fields[] = { "timestep", "wxyz", "axis_coeff", "angle_coeff", "link", "tolerance", "axis", "tcp_" + "wxyz" }; + ensure_only_members(params, all_fields, sizeof(all_fields) / sizeof(char*)); } // add the term to the problem description -void AlignedAxisTermInfo::hatch(TrajOptProb& prob) { +void AlignedAxisTermInfo::hatch(TrajOptProb& prob) +{ // construct the desired pose Eigen::Quaterniond input_q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); Eigen::Quaterniond tcp_q(tcp_wxyz(0), tcp_wxyz(1), tcp_wxyz(2), tcp_wxyz(3)); // create the equality and inequality error functions to be used - VectorOfVectorPtr f_ineq(new AlignedAxisErrCalculator(input_q.matrix(), prob.GetKin(), prob.GetEnv(), - link, axis, tolerance, tcp_q.matrix())); + VectorOfVectorPtr f_ineq(new AlignedAxisErrCalculator( + input_q.matrix(), prob.GetKin(), prob.GetEnv(), link, axis, tolerance, tcp_q.matrix())); Eigen::Vector4d coeffs(axis_coeff, axis_coeff, axis_coeff, angle_coeff); // add the costs or constraints depending on the term type - if (term_type == TT_COST) { + if (term_type == TT_COST) + { prob.addCost(CostPtr(new CostFromErrFunc(f_ineq, prob.GetVarRow(timestep), coeffs, HINGE, name))); } - else if (term_type == TT_CNT) { + else if (term_type == TT_CNT) + { prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_ineq, prob.GetVarRow(timestep), coeffs, INEQ, name))); } } // initialize with basic default values -ConicalAxisTermInfo::ConicalAxisTermInfo() : tcp_wxyz(1.0, 0.0, 0.0, 0.0) { - weight = 0.0; -} - +ConicalAxisTermInfo::ConicalAxisTermInfo() : tcp_wxyz(1.0, 0.0, 0.0, 0.0) { weight = 0.0; } // construct the term from a Json file -void ConicalAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value &v) { +void ConicalAxisTermInfo::fromJson(ProblemConstructionInfo& pci, const Value& v) +{ // make sure params is a member of the Json value provided FAIL_IF_FALSE(v.isMember("params")); // get the params value and read the parameters fmro the file into the object members const Value& params = v["params"]; - childFromJson(params, timestep, "timestep", pci.basic_info.n_steps-1); + childFromJson(params, timestep, "timestep", pci.basic_info.n_steps - 1); childFromJson(params, wxyz, "wxyz"); childFromJson(params, tcp_wxyz, "tcp_xwyz", tcp_wxyz); childFromJson(params, weight, "weight"); @@ -882,37 +885,38 @@ void ConicalAxisTermInfo::fromJson(ProblemConstructionInfo &pci, const Value &v) // make sure the link name provided is valid const std::vector& link_names = pci.kin->getLinkNames(); - if (std::find(link_names.begin(), link_names.end(),link)==link_names.end()) { - PRINT_AND_THROW(boost::format("invalid link name: %s")%link); + if (std::find(link_names.begin(), link_names.end(), link) == link_names.end()) + { + PRINT_AND_THROW(boost::format("invalid link name: %s") % link); } // make sure there are no extra items in the params value - const char* all_fields[] = {"timestep", "wxyz", "weight", "link", - "tolerance", "axis", "tcp_wxyz"}; - ensure_only_members(params, all_fields, sizeof(all_fields)/sizeof(char*)); + const char* all_fields[] = { "timestep", "wxyz", "weight", "link", "tolerance", "axis", "tcp_wxyz" }; + ensure_only_members(params, all_fields, sizeof(all_fields) / sizeof(char*)); } // add the term to the problem description -void ConicalAxisTermInfo::hatch(TrajOptProb &prob) { +void ConicalAxisTermInfo::hatch(TrajOptProb& prob) +{ // construct the desired pose Eigen::Quaterniond input_q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); Eigen::Quaterniond tcp_q(tcp_wxyz(0), tcp_wxyz(1), tcp_wxyz(2), tcp_wxyz(3)); // create the equality and inequality error functions to be used - VectorOfVectorPtr f_ineq(new ConicalAxisErrCalculator(input_q.matrix(), prob.GetKin(), prob.GetEnv(), - link, axis, tolerance, tcp_q.matrix())); + VectorOfVectorPtr f_ineq(new ConicalAxisErrCalculator( + input_q.matrix(), prob.GetKin(), prob.GetEnv(), link, axis, tolerance, tcp_q.matrix())); VectorXd coeffs(1); coeffs(0) = weight; // add the costs or constraints depending on the term type - if (term_type == TT_COST) { + if (term_type == TT_COST) + { prob.addCost(CostPtr(new CostFromErrFunc(f_ineq, prob.GetVarRow(timestep), coeffs, HINGE, name))); } - else if (term_type == TT_CNT) { + else if (term_type == TT_CNT) + { prob.addConstraint(ConstraintPtr(new ConstraintFromFunc(f_ineq, prob.GetVarRow(timestep), coeffs, INEQ, name))); } - } - } diff --git a/trajopt/test/angular_constraint_unit.cpp b/trajopt/test/angular_constraint_unit.cpp index 724e0910..ee9fb9f4 100644 --- a/trajopt/test/angular_constraint_unit.cpp +++ b/trajopt/test/angular_constraint_unit.cpp @@ -23,32 +23,34 @@ using namespace tesseract; using namespace Eigen; const std::string ROBOT_DESCRIPTION_PARAM = "robot_description"; /**< Default ROS parameter for robot description */ -const std::string ROBOT_SEMANTIC_PARAM = "robot_description_semantic"; /**< Default ROS parameter for robot description */ +const std::string ROBOT_SEMANTIC_PARAM = + "robot_description_semantic"; /**< Default ROS parameter for robot description */ bool plotting_ = false; int steps_ = 5; double approx_equals_diff_ = 1e-2; -struct testInfo { +struct testInfo +{ std::string method; std::string constraint; testInfo(std::string m, std::string c) : method(m), constraint(c) {} }; -class AngularConstraintTest : public testing::TestWithParam { +class AngularConstraintTest : public testing::TestWithParam +{ public: ros::NodeHandle nh_; urdf::ModelInterfaceSharedPtr urdf_model_; /**< URDF Model */ srdf::ModelSharedPtr srdf_model_; /**< SRDF Model */ - tesseract_ros::KDLEnvPtr env_; /**< Trajopt Basic Environment */ + tesseract_ros::KDLEnvPtr env_; /**< Trajopt Basic Environment */ tesseract_ros::ROSBasicPlottingPtr plotter_; /**< Trajopt Plotter */ std::vector pose_inverses_; std::string constraint_type_; double tol_; AngularConstraintTest() : pose_inverses_(steps_) {} - TrajOptProbPtr jsonMethod() { std::string config_file; @@ -64,7 +66,7 @@ class AngularConstraintTest : public testing::TestWithParam { ProblemConstructionInfo pci(env_); pci.fromJson(root); - double y[] = {-0.2, -0.1, 0.0, 0.1, 0.2}; + double y[] = { -0.2, -0.1, 0.0, 0.1, 0.2 }; for (int i = 0; i < 5; i++) { Vector3d xyz(0.5, y[i], 0.62); @@ -77,7 +79,6 @@ class AngularConstraintTest : public testing::TestWithParam { pose_inverses_.insert(pose_inverses_.begin() + i, cur_pose.inverse()); } - return ConstructProblem(pci); } @@ -89,7 +90,7 @@ class AngularConstraintTest : public testing::TestWithParam { pci.basic_info.n_steps = steps_; pci.basic_info.manip = "manipulator"; pci.basic_info.start_fixed = false; - // pci.basic_info.dofs_fixed + // pci.basic_info.dofs_fixed // Create Kinematic Object pci.kin = pci.env->getManipulator(pci.basic_info.manip); @@ -118,7 +119,7 @@ class AngularConstraintTest : public testing::TestWithParam { pci.cost_infos.push_back(collision); // Populate Constraints - double delta = 0.5/pci.basic_info.n_steps; + double delta = 0.5 / pci.basic_info.n_steps; for (auto i = 0; i < pci.basic_info.n_steps; ++i) { Vector3d xyz(0.5, -0.2 + delta * i, 0.62); @@ -142,7 +143,8 @@ class AngularConstraintTest : public testing::TestWithParam { position->rot_coeffs = Eigen::Vector3d::Zero(); pci.cnt_infos.push_back(position); - if (constraint_type_ == "aligned") { + if (constraint_type_ == "aligned") + { std::shared_ptr aligned(new AlignedAxisTermInfo); aligned->tolerance = tol_; aligned->axis = Vector3d::UnitX(); @@ -155,7 +157,8 @@ class AngularConstraintTest : public testing::TestWithParam { aligned->angle_coeff = 10.0; pci.cnt_infos.push_back(aligned); } - else if (constraint_type_ == "conical") { + else if (constraint_type_ == "conical") + { std::shared_ptr conical(new ConicalAxisTermInfo); conical->tolerance = tol_; conical->axis = Vector3d::UnitZ(); @@ -167,11 +170,11 @@ class AngularConstraintTest : public testing::TestWithParam { conical->weight = 10.0; pci.cnt_infos.push_back(conical); } - else { + else + { ROS_ERROR("%s is not a valid constraint type. Exiting", constraint_type_.c_str()); exit(-1); } - } return ConstructProblem(pci); @@ -179,7 +182,6 @@ class AngularConstraintTest : public testing::TestWithParam { virtual void SetUp() { - // Initial setup std::string urdf_xml_string, srdf_xml_string; nh_.getParam(ROBOT_DESCRIPTION_PARAM, urdf_xml_string); @@ -197,20 +199,20 @@ class AngularConstraintTest : public testing::TestWithParam { pcl::PointCloud full_cloud; double delta = 0.05; - int length = (1/delta); + int length = (1 / delta); for (int x = 0; x < length; ++x) for (int y = 0; y < length; ++y) for (int z = 0; z < length; ++z) - full_cloud.push_back(pcl::PointXYZ(-0.5 + x*delta, -0.5 + y*delta, -0.5 + z*delta)); + full_cloud.push_back(pcl::PointXYZ(-0.5 + x * delta, -0.5 + y * delta, -0.5 + z * delta)); sensor_msgs::PointCloud2 pointcloud_msg; pcl::toROSMsg(full_cloud, pointcloud_msg); octomap::Pointcloud octomap_data; octomap::pointCloud2ToOctomap(pointcloud_msg, octomap_data); - octomap::OcTree* octree = new octomap::OcTree(2*delta); - octree->insertPointCloud(octomap_data, octomap::point3d(0,0,0)); + octomap::OcTree* octree = new octomap::OcTree(2 * delta); + octree->insertPointCloud(octomap_data, octomap::point3d(0, 0, 0)); AttachableObjectPtr obj(new AttachableObject()); shapes::OcTree* octomap_world = new shapes::OcTree(std::shared_ptr(octree)); @@ -247,7 +249,6 @@ class AngularConstraintTest : public testing::TestWithParam { // Set Log Level gLogLevel = util::LevelError; - } }; @@ -260,16 +261,21 @@ TEST_P(AngularConstraintTest, AngularConstraint) constraint_type_ = info.constraint; int num_iterations = method == "cpp" ? 3 : 1; - for (int i = 0; i < num_iterations; i++) { - tol_ = (method == "cpp" ? 5.0 * (i + 1) : 5.0) * M_PI/180.0;; + for (int i = 0; i < num_iterations; i++) + { + tol_ = (method == "cpp" ? 5.0 * (i + 1) : 5.0) * M_PI / 180.0; + ; - if (method == "cpp") { + if (method == "cpp") + { prob = cppMethod(); } - else if (method == "json") { + else if (method == "json") + { prob = jsonMethod(); } - else { + else + { ROS_ERROR("Method string invalid. Exiting"); exit(-1); } @@ -311,10 +317,12 @@ TEST_P(AngularConstraintTest, AngularConstraint) tcp.setIdentity(); TrajArray traj = getTraj(opt.x(), prob->GetVars()); - for (auto j = 0; j < traj.rows(); j++) { + for (auto j = 0; j < traj.rows(); j++) + { VectorXd joint_angles(traj.cols()); - for (auto k = 0; k < traj.cols(); k++) { - joint_angles(k) = traj(j,k); + for (auto k = 0; k < traj.cols(); k++) + { + joint_angles(k) = traj(j, k); } Affine3d pose; @@ -323,28 +331,32 @@ TEST_P(AngularConstraintTest, AngularConstraint) Affine3d pose_inv = pose_inverses_.at(j); Affine3d err = pose_inv * (pose * tcp); - if (constraint_type_ == "aligned") { + if (constraint_type_ == "aligned") + { AngleAxisd aa(err.rotation()); double aligned_angle = aa.angle(); - EXPECT_LE(aligned_angle, tol_ * (1.0 + approx_equals_diff_) ); + EXPECT_LE(aligned_angle, tol_ * (1.0 + approx_equals_diff_)); Vector3d axis = aa.axis(); double dot_prod = axis.dot(Vector3d::UnitX()); EXPECT_NEAR(1.0, fabs(dot_prod), approx_equals_diff_); } - else { // conical + else + { // conical Matrix3d orientation(err.rotation()); double conical_angle = acos(orientation(2, 2)); EXPECT_LE(conical_angle, tol_ * (1 + approx_equals_diff_)); } } - } - } -INSTANTIATE_TEST_CASE_P(Tests, AngularConstraintTest, ::testing::Values(testInfo("json", "aligned"), - testInfo("json", "conical"), testInfo("cpp", "aligned"), testInfo("cpp", "conical")) ); +INSTANTIATE_TEST_CASE_P(Tests, + AngularConstraintTest, + ::testing::Values(testInfo("json", "aligned"), + testInfo("json", "conical"), + testInfo("cpp", "aligned"), + testInfo("cpp", "conical"))); int main(int argc, char** argv) { @@ -356,7 +368,4 @@ int main(int argc, char** argv) pnh.param("plotting", plotting_, plotting_); return RUN_ALL_TESTS(); - } - -