Skip to content

Commit

Permalink
Add term_type switch for time parameterization
Browse files Browse the repository at this point in the history
  • Loading branch information
@mpowelson
mpowelson committed Dec 7, 2018
1 parent 9445d52 commit 8a72931
Show file tree
Hide file tree
Showing 5 changed files with 301 additions and 88 deletions.
75 changes: 56 additions & 19 deletions trajopt/include/trajopt/problem_description.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,14 +23,15 @@ struct ProblemConstructionInfo;
struct TrajOptResult;
typedef std::shared_ptr<TrajOptResult> TrajOptResultPtr;

TrajOptProbPtr TRAJOPT_API ConstructProblem(const ProblemConstructionInfo&);
TrajOptProbPtr TRAJOPT_API ConstructProblem(const ProblemConstructionInfo &);
TrajOptProbPtr TRAJOPT_API ConstructProblem(const Json::Value&, tesseract::BasicEnvConstPtr env);
TrajOptResultPtr TRAJOPT_API OptimizeProblem(TrajOptProbPtr, const tesseract::BasicPlottingPtr plotter = nullptr);

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,15 @@ class TRAJOPT_API TrajOptProb : public sco::OptProb
TrajOptProb();
TrajOptProb(int n_steps, const ProblemConstructionInfo& pci);
~TrajOptProb() {}
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,6 +70,8 @@ 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&);
// Indicates whether or not the last column is dt
bool has_time;

private:
VarArray m_traj_vars;
Expand Down Expand Up @@ -87,6 +99,10 @@ struct BasicInfo
std::string robot; // optional
IntVec dofs_fixed; // optional
sco::ModelType convex_solver; // which convex solver to use

// optional, but must be valid
double dt_upper_lim = 1.0;
double dt_lower_lim = 1.0;
};

/**
Expand All @@ -97,10 +113,13 @@ struct InitInfo
enum Type
{
STATIONARY,
JOINT_INTERPOLATED,
GIVEN_TRAJ,
};
Type type;
TrajArray data;
bool use_time = false;
double dt = 1.0;
};

struct TRAJOPT_API MakesCost
Expand All @@ -117,8 +136,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 +154,12 @@ struct TRAJOPT_API TermInfo

virtual ~TermInfo() {}

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 +189,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 +216,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 +248,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 +260,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);
DEFINE_CREATE(CartVelTermInfo)

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

/**
Expand All @@ -247,7 +274,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 +293,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);
DEFINE_CREATE(JointPosTermInfo)

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

/**
Expand All @@ -284,8 +313,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 +323,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 +342,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);
DEFINE_CREATE(JointVelTermInfo)

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

/**
Expand All @@ -331,12 +362,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 +386,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);
DEFINE_CREATE(JointAccTermInfo)

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

/**
Expand All @@ -373,12 +406,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 +430,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);
DEFINE_CREATE(JointJerkTermInfo)

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

/**
Expand All @@ -411,7 +446,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 +467,8 @@ struct CollisionTermInfo : public TermInfo, public MakesCost
/** @brief Converts term info into cost/constraint and adds it to trajopt problem */
void hatch(TrajOptProb& prob);
DEFINE_CREATE(CollisionTermInfo)

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

} // namespace trajopt
Loading

0 comments on commit 8a72931

Please sign in to comment.