Skip to content
New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Integrate time param - term type switch #50

Merged
merged 5 commits into from
Dec 17, 2018
Merged

Integrate time param - term type switch #50

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
b89b2e5
Add term_type switch for time parameterization
mpowelson Dec 7, 2018
e0ef863
Add initial trajectory unit tests
mpowelson Dec 10, 2018
bd3c79f
Add unit tests to initial trajectory via json and other fixes
mpowelson Dec 11, 2018
38ad0ab
Add check that costs/cnts are pushed to correct term info
mpowelson Dec 12, 2018
90208b2
Replace GetJointVarRow with GetVarRow
mpowelson Dec 14, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions trajopt/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,9 @@ if (CATKIN_ENABLE_TESTING)
add_rostest_gtest(${PROJECT_NAME}_planning_unit test/planning_unit.launch test/planning_unit.cpp)
target_link_libraries(${PROJECT_NAME}_planning_unit ${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${catkin_LIBRARIES})

add_rostest_gtest(${PROJECT_NAME}_interface_unit test/interface_unit.launch test/interface_unit.cpp)
target_link_libraries(${PROJECT_NAME}_interface_unit ${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${catkin_LIBRARIES})

add_rostest_gtest(${PROJECT_NAME}_costs_unit test/costs_unit.launch test/costs_unit.cpp)
target_link_libraries(${PROJECT_NAME}_costs_unit ${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${catkin_LIBRARIES})

Expand Down
98 changes: 77 additions & 21 deletions trajopt/include/trajopt/problem_description.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,9 @@ TrajOptResultPtr TRAJOPT_API OptimizeProblem(TrajOptProbPtr, const tesseract::Ba

enum TermType
{
TT_COST,
TT_CNT
TT_COST = 0x1, // 0000 0001
TT_CNT = 0x2, // 0000 0010
TT_USE_TIME = 0x4, // 0000 0100
};

#define DEFINE_CREATE(classname) \
Expand All @@ -50,6 +51,14 @@ class TRAJOPT_API TrajOptProb : public sco::OptProb
TrajOptProb();
TrajOptProb(int n_steps, const ProblemConstructionInfo& pci);
virtual ~TrajOptProb() = default;
sco::VarVector GetJointVarRow(int i)
{
sco::VarVector var_row = GetVarRow(i);
// Remove time column
if (has_time)
var_row.pop_back();
return var_row;
}
sco::VarVector GetVarRow(int i) { return m_traj_vars.row(i); }
sco::Var& GetVar(int i, int j) { return m_traj_vars.at(i, j); }
VarArray& GetVars() { return m_traj_vars; }
Expand All @@ -60,8 +69,15 @@ class TRAJOPT_API TrajOptProb : public sco::OptProb
void SetInitTraj(const TrajArray& x) { m_init_traj = x; }
TrajArray GetInitTraj() { return m_init_traj; }
friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&);
/** @brief Returns TrajOptProb.has_time */
bool GetHasTime() { return has_time;}
/** @brief Sets TrajOptProb.has_time */
void SetHasTime(bool tmp) { has_time = tmp;}


private:
/** @brief If true, the last column in the optimization matrix will be 1/dt */
bool has_time;
VarArray m_traj_vars;
tesseract::BasicKinConstPtr m_kin;
tesseract::BasicEnvConstPtr m_env;
Expand All @@ -81,26 +97,48 @@ struct TRAJOPT_API TrajOptResult

struct BasicInfo
{
/** @brief If true first time step is fixed with a joint level constraint*/
bool start_fixed;
/** @brief Number of time steps (rows) in the optimization matrix */
int n_steps;
std::string manip;
std::string robot; // optional
IntVec dofs_fixed; // optional
sco::ModelType convex_solver; // which convex solver to use
std::string robot; // optional
IntVec dofs_fixed; // optional
sco::ModelType convex_solver; // which convex solver to use

/** @brief If true, the last column in the optimization matrix will be 1/dt */
bool use_time = false;
/** @brief The upper limit of 1/dt values allowed in the optimization*/
double dt_upper_lim = 1.0;
/** @brief The lower limit of 1/dt values allowed in the optimization*/
double dt_lower_lim = 1.0;
};

/**
Initialization info read from json
*/
struct InitInfo
{
/** @brief Methods of initializing the optimization matrix

STATIONARY: Initializes all joint values to the initial value (the current value in the env pci.env->getCurrentJointValues)
JOINT_INTERPOLATED: Linearly interpolates between initial value and the joint position specified in InitInfo.data
GIVEN_TRAJ: Initializes the matrix to a given trajectory

In all cases the dt column (if present) is appended the selected method is defined.
*/
enum Type
{
STATIONARY,
JOINT_INTERPOLATED,
GIVEN_TRAJ,
};
/** @brief Specifies the type of initialization to use */
Type type;
/** @brief Data used during initialization. Use depends on the initialization selected. */
TrajArray data;
/** @brief Default value the final column of the optimization is initialized too if time is being used */
double dt = 1.0;
};

struct TRAJOPT_API MakesCost
Expand All @@ -117,8 +155,10 @@ Then it later gets converted to a Cost object by the hatch method
*/
struct TRAJOPT_API TermInfo
{
std::string name; // xxx is this used?
TermType term_type;
std::string name;
int term_type;
int GetSupportedTypes() { return supported_term_type_; }

virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0;
virtual void hatch(TrajOptProb& prob) = 0;

Expand All @@ -133,8 +173,12 @@ struct TRAJOPT_API TermInfo

virtual ~TermInfo() = default;

protected:
TermInfo(int supported_term_type) : supported_term_type_(supported_term_type) {}

private:
static std::map<std::string, MakerFunc> name2maker;
int supported_term_type_;
};

/**
Expand Down Expand Up @@ -164,7 +208,7 @@ struct TRAJOPT_API ProblemConstructionInfo
};

/** @brief This is used when the goal frame is not fixed in space */
struct DynamicCartPoseTermInfo : public TermInfo, public MakesCost, public MakesConstraint
struct DynamicCartPoseTermInfo : public TermInfo
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Expand All @@ -191,7 +235,7 @@ struct DynamicCartPoseTermInfo : public TermInfo, public MakesCost, public Makes

Set term_type == TT_COST or TT_CNT for cost or constraint.
*/
struct CartPoseTermInfo : public TermInfo, public MakesCost, public MakesConstraint
struct CartPoseTermInfo : public TermInfo
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Expand Down Expand Up @@ -223,7 +267,7 @@ struct CartPoseTermInfo : public TermInfo, public MakesCost, public MakesConstra
Constrains the change in position of the link in each timestep to be less than
max_displacement
*/
struct CartVelTermInfo : public TermInfo, public MakesCost, public MakesConstraint
struct CartVelTermInfo : public TermInfo
{
/** @brief Timesteps over which to apply term */
int first_step, last_step;
Expand All @@ -235,6 +279,8 @@ struct CartVelTermInfo : public TermInfo, public MakesCost, public MakesConstrai
/** @brief Converts term info into cost/constraint and adds it to trajopt problem */
void hatch(TrajOptProb& prob) override;
DEFINE_CREATE(CartVelTermInfo)

CartVelTermInfo() : TermInfo(TT_COST | TT_CNT) {}
};

/**
Expand All @@ -247,7 +293,7 @@ struct CartVelTermInfo : public TermInfo, public MakesCost, public MakesConstrai
\f}
where \f$i\f$ indexes over dof and \f$c_i\f$ are coeffs
*/
struct JointPosTermInfo : public TermInfo, public MakesCost, public MakesConstraint
struct JointPosTermInfo : public TermInfo
{
/** @brief Vector of coefficients that scales cost. */
DblVec coeffs;
Expand All @@ -266,6 +312,8 @@ struct JointPosTermInfo : public TermInfo, public MakesCost, public MakesConstra
/** @brief Converts term info into cost/constraint and adds it to trajopt problem */
void hatch(TrajOptProb& prob) override;
DEFINE_CREATE(JointPosTermInfo)

JointPosTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) {}
};

/**
Expand All @@ -284,8 +332,8 @@ velocity > lower_limit, no penalty.
** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when velocity <
upper_limit and velocity > lower_limit

Note: targets, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints. If
one value is given, this will be broadcast to all joints.
Note: targets, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints.
If one value is given, this will be broadcast to all joints.

Note: Velocity is calculated numerically using forward finite difference

Expand All @@ -294,7 +342,7 @@ Note: Velocity is calculated numerically using forward finite difference
\f}
where j indexes over DOF, and \f$c_j\f$ are the coeffs.
*/
struct JointVelTermInfo : public TermInfo, public MakesCost, public MakesConstraint
struct JointVelTermInfo : public TermInfo
{
/** @brief Vector of coefficients that scales cost for each joint. */
DblVec coeffs;
Expand All @@ -313,6 +361,8 @@ struct JointVelTermInfo : public TermInfo, public MakesCost, public MakesConstra
/** @brief Converts term info into cost/constraint and adds it to trajopt problem */
void hatch(TrajOptProb& prob) override;
DEFINE_CREATE(JointVelTermInfo)

JointVelTermInfo() : TermInfo(TT_COST | TT_CNT) {}
};

/**
Expand All @@ -331,12 +381,12 @@ and acceleration > lower_limit, no penalty.
** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when acceleration <
upper_limit and acceleration > lower_limit

Note: targets, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints. If
one value is given, this will be broadcast to all joints.
Note: targets, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints.
If one value is given, this will be broadcast to all joints.

Note: Acceleration is calculated numerically using central finite difference
*/
struct JointAccTermInfo : public TermInfo, public MakesCost, public MakesConstraint
struct JointAccTermInfo : public TermInfo
{
/** @brief For TT_COST: coefficient that scales cost. For TT_CNT: Acceleration limit*/
DblVec coeffs;
Expand All @@ -355,6 +405,8 @@ struct JointAccTermInfo : public TermInfo, public MakesCost, public MakesConstra
/** @brief Converts term info into cost/constraint and adds it to trajopt problem */
void hatch(TrajOptProb& prob) override;
DEFINE_CREATE(JointAccTermInfo)

JointAccTermInfo() : TermInfo(TT_COST | TT_CNT) {}
};

/**
Expand All @@ -373,12 +425,12 @@ jerk > lower_limit, no penalty.
** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when jerk <
upper_limit and jerk > lower_limit

Note: targets, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints. If
one value is given, this will be broadcast to all joints.
Note: targets, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints.
If one value is given, this will be broadcast to all joints.

Note: Jerk is calculated numerically using central finite difference
*/
struct JointJerkTermInfo : public TermInfo, public MakesCost, public MakesConstraint
struct JointJerkTermInfo : public TermInfo
{
/** @brief For TT_COST: coefficient that scales cost. For TT_CNT: Jerk limit */
DblVec coeffs;
Expand All @@ -397,6 +449,8 @@ struct JointJerkTermInfo : public TermInfo, public MakesCost, public MakesConstr
/** @brief Converts term info into cost/constraint and adds it to trajopt problem */
void hatch(TrajOptProb& prob) override;
DEFINE_CREATE(JointJerkTermInfo)

JointJerkTermInfo() : TermInfo(TT_COST | TT_CNT) {}
};

/**
Expand All @@ -411,7 +465,7 @@ Continuous-time penalty: same, except you consider swept-out shaps of robot
links. Currently self-collisions are not included.

*/
struct CollisionTermInfo : public TermInfo, public MakesCost
struct CollisionTermInfo : public TermInfo
{
/** @brief first_step and last_step are inclusive */
int first_step, last_step;
Expand All @@ -432,6 +486,8 @@ struct CollisionTermInfo : public TermInfo, public MakesCost
/** @brief Converts term info into cost/constraint and adds it to trajopt problem */
void hatch(TrajOptProb& prob) override;
DEFINE_CREATE(CollisionTermInfo)

CollisionTermInfo() : TermInfo(TT_COST) {}
};

} // namespace trajopt
Loading