Skip to content

Commit bf4a31c

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Implement the rest of the joint constraints for new variable set
1 parent 451343e commit bf4a31c

File tree

8 files changed

+867
-27
lines changed

8 files changed

+867
-27
lines changed

‎trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h‎

Lines changed: 79 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
3333
namespace trajopt_ifopt
3434
{
3535
class JointPosition;
36-
36+
class Var;
3737
/**
3838
* @brief This creates a joint acceleration constraint and allows bounds to be set on a joint position
3939
*
@@ -108,5 +108,83 @@ class JointAccelConstraint : public ifopt::ConstraintSet
108108
std::vector<std::shared_ptr<const JointPosition>> position_vars_;
109109
std::unordered_map<std::string, Eigen::Index> index_map_;
110110
};
111+
112+
/**
113+
* @brief This creates a joint acceleration constraint and allows bounds to be set on a joint position
114+
*
115+
* Joint acceleration is calculated as a = th_2 - 2th_1 + th_0
116+
*/
117+
class JointAccelConstraint2 : public ifopt::ConstraintSet
118+
{
119+
public:
120+
using Ptr = std::shared_ptr<JointAccelConstraint>;
121+
using ConstPtr = std::shared_ptr<const JointAccelConstraint>;
122+
123+
/**
124+
* @brief Constructs a acceleration contraint from these variables, setting the bounds to the target
125+
* @param targets Joint Acceleration targets (length should be n_dof). Upper and lower bounds are set to this value
126+
* @param position_vars Joint positions used to calculate acceleration. These vars are assumed to be continuous and in
127+
* order.
128+
* @param coeffs The joint coefficients to use as weights. If size of 1 then the values is replicated for each joint.
129+
* @param name Name of the constraint
130+
*/
131+
JointAccelConstraint2(const Eigen::VectorXd& targets,
132+
const std::vector<std::shared_ptr<const Var>>& position_vars,
133+
const Eigen::VectorXd& coeffs,
134+
const std::string& name = "JointAccel");
135+
136+
/**
137+
* @brief Constructs a acceleration contraint from these variables, setting the bounds to those passed in.
138+
* @param bounds Bounds on target joint acceleration (length should be n_dof)
139+
* @param position_vars Joint positions used to calculate acceleration. These vars are assumed to be continuous and in
140+
* order.
141+
* @param coeffs The joint coefficients to use as weights. If size of 1 then the values is replicated for each joint.
142+
* @param name Name of the constraint
143+
*/
144+
JointAccelConstraint2(const std::vector<ifopt::Bounds>& bounds,
145+
const std::vector<std::shared_ptr<const JointPosition>>& position_vars,
146+
const Eigen::VectorXd& coeffs,
147+
const std::string& name = "JointAccel");
148+
149+
/**
150+
* @brief Returns the values associated with the constraint. In this case that is the approximate joint acceleration.
151+
* @return Returns jointAcceleration. Length is n_dof_ * n_vars
152+
*/
153+
Eigen::VectorXd GetValues() const override;
154+
155+
/**
156+
* @brief Returns the "bounds" of this constraint. How these are enforced is up to the solver
157+
* @return Returns the "bounds" of this constraint
158+
*/
159+
std::vector<ifopt::Bounds> GetBounds() const override;
160+
161+
/**
162+
* @brief Fills the jacobian block associated with the given var_set.
163+
* @param var_set Name of the var_set to which the jac_block is associated
164+
* @param jac_block Block of the overall jacobian associated with these constraints and the var_set variable
165+
*/
166+
void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const override;
167+
168+
private:
169+
/** @brief The var_set associated with this constraint */
170+
std::string var_set_;
171+
172+
/** @brief The number of joints in a single JointPosition */
173+
long n_dof_;
174+
175+
/** @brief The number of JointPositions passed in */
176+
long n_vars_;
177+
178+
/** @brief The coeff to apply to error and gradient */
179+
Eigen::VectorXd coeffs_;
180+
181+
/** @brief Bounds on the velocities of each joint */
182+
std::vector<ifopt::Bounds> bounds_;
183+
184+
/** @brief Pointers to the vars used by this constraint.
185+
*
186+
* Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/
187+
std::vector<std::shared_ptr<const Var>> position_vars_;
188+
};
111189
} // namespace trajopt_ifopt
112190
#endif

‎trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
3333
namespace trajopt_ifopt
3434
{
3535
class JointPosition;
36+
class Var;
3637

3738
/**
3839
* @brief This creates a joint acceleration constraint and allows bounds to be set on a joint position
@@ -108,5 +109,83 @@ class JointJerkConstraint : public ifopt::ConstraintSet
108109
std::vector<std::shared_ptr<const JointPosition>> position_vars_;
109110
std::unordered_map<std::string, Eigen::Index> index_map_;
110111
};
112+
113+
/**
114+
* @brief This creates a joint acceleration constraint and allows bounds to be set on a joint position
115+
*
116+
* Joint acceleration is calculated as a = th_2 - 2th_1 + th_0
117+
*/
118+
class JointJerkConstraint2 : public ifopt::ConstraintSet
119+
{
120+
public:
121+
using Ptr = std::shared_ptr<JointJerkConstraint>;
122+
using ConstPtr = std::shared_ptr<const JointJerkConstraint>;
123+
124+
/**
125+
* @brief Constructs a jerk constraint from these variables, setting the bounds to the target
126+
* @param targets Joint Jerk targets (length should be n_dof). Upper and lower bounds are set to this value
127+
* @param position_vars Joint positions used to calculate acceleration. These vars are assumed to be continuous and in
128+
* order.
129+
* @param coeffs The joint coefficients to use as weights. If size of 1 then the values is replicated for each joint.
130+
* @param name Name of the constraint
131+
*/
132+
JointJerkConstraint2(const Eigen::VectorXd& targets,
133+
const std::vector<std::shared_ptr<const Var>>& position_vars,
134+
const Eigen::VectorXd& coeffs,
135+
const std::string& name = "JointJerk");
136+
137+
/**
138+
* @brief Constructs a jerk constraint from these variables, setting the bounds to those passed in.
139+
* @param bounds Bounds on target joint acceleration (length should be n_dof)
140+
* @param position_vars Joint positions used to calculate acceleration. These vars are assumed to be continuous and in
141+
* order.
142+
* @param coeffs The joint coefficients to use as weights. If size of 1 then the values is replicated for each joint.
143+
* @param name Name of the constraint
144+
*/
145+
JointJerkConstraint2(const std::vector<ifopt::Bounds>& bounds,
146+
const std::vector<std::shared_ptr<const JointPosition>>& position_vars,
147+
const Eigen::VectorXd& coeffs,
148+
const std::string& name = "JointJerk");
149+
150+
/**
151+
* @brief Returns the values associated with the constraint. In this case that is the approximate joint jerk.
152+
* @return Returns joint jerk. Length is n_dof_ * n_vars
153+
*/
154+
Eigen::VectorXd GetValues() const override;
155+
156+
/**
157+
* @brief Returns the "bounds" of this constraint. How these are enforced is up to the solver
158+
* @return Returns the "bounds" of this constraint
159+
*/
160+
std::vector<ifopt::Bounds> GetBounds() const override;
161+
162+
/**
163+
* @brief Fills the jacobian block associated with the given var_set.
164+
* @param var_set Name of the var_set to which the jac_block is associated
165+
* @param jac_block Block of the overall jacobian associated with these constraints and the var_set variable
166+
*/
167+
void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const override;
168+
169+
private:
170+
/** @brief The var_set associated with this constraint */
171+
std::string var_set_;
172+
173+
/** @brief The number of joints in a single JointPosition */
174+
long n_dof_;
175+
176+
/** @brief The number of JointPositions passed in */
177+
long n_vars_;
178+
179+
/** @brief The coeff to apply to error and gradient */
180+
Eigen::VectorXd coeffs_;
181+
182+
/** @brief Bounds on the velocities of each joint */
183+
std::vector<ifopt::Bounds> bounds_;
184+
185+
/** @brief Pointers to the vars used by this constraint.
186+
*
187+
* Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/
188+
std::vector<std::shared_ptr<const Var>> position_vars_;
189+
};
111190
} // namespace trajopt_ifopt
112191
#endif

‎trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h‎

Lines changed: 74 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
3333
namespace trajopt_ifopt
3434
{
3535
class JointPosition;
36-
36+
class Var;
3737
/**
3838
* @brief This creates a joint position constraint. Allows bounds to be set on a joint position
3939
*/
@@ -105,5 +105,78 @@ class JointPosConstraint : public ifopt::ConstraintSet
105105
* Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/
106106
std::vector<std::shared_ptr<const JointPosition>> position_vars_;
107107
};
108+
109+
/**
110+
* @brief This creates a joint position constraint. Allows bounds to be set on a joint position
111+
*/
112+
class JointPosConstraint2 : public ifopt::ConstraintSet
113+
{
114+
public:
115+
using Ptr = std::shared_ptr<JointPosConstraint>;
116+
using ConstPtr = std::shared_ptr<const JointPosConstraint>;
117+
118+
/**
119+
* @brief JointPosConstraint
120+
* @param targets Target joint position (length should be n_dof). Upper and lower bounds are set to this value
121+
* @param position_vars Variables to which this constraint is applied. Note that all variables should have the same
122+
* number of components (joint DOF)
123+
* @param coeffs The joint coefficients to use as weights. If size of 1 then the values is replicated for each joint.
124+
* @param name Name of the constraint
125+
*/
126+
JointPosConstraint2(const Eigen::VectorXd& target,
127+
const std::shared_ptr<const Var>& position_var,
128+
const Eigen::VectorXd& coeffs,
129+
const std::string& name = "JointPos");
130+
131+
/**
132+
* @brief JointPosConstraint
133+
* @param bounds Bounds on target joint position (length should be n_dof)
134+
* @param position_vars Variables to which this constraint is applied
135+
* @param coeffs The joint coefficients to use as weights. If size of 1 then the values is replicated for each joint.
136+
* @param name Name of the constraint
137+
*/
138+
JointPosConstraint2(const std::vector<ifopt::Bounds>& bounds,
139+
const std::shared_ptr<const Var>& position_vars,
140+
const Eigen::VectorXd& coeffs,
141+
const std::string& name = "JointPos");
142+
143+
/**
144+
* @brief Returns the values associated with the constraint. In this case that is the concatenated joint values
145+
* associated with each of the joint positions should be n_dof_ * n_vars_ long
146+
* @return
147+
*/
148+
Eigen::VectorXd GetValues() const override;
149+
150+
/**
151+
* @brief Returns the "bounds" of this constraint. How these are enforced is up to the solver
152+
* @return Returns the "bounds" of this constraint
153+
*/
154+
std::vector<ifopt::Bounds> GetBounds() const override;
155+
156+
/**
157+
* @brief Fills the jacobian block associated with the given var_set.
158+
* @param var_set Name of the var_set to which the jac_block is associated
159+
* @param jac_block Block of the overal jacobian associated with these constraints and the var_set variable
160+
*/
161+
void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const override;
162+
163+
private:
164+
/** @brief The var_set associated with this constraint */
165+
std::string var_set_;
166+
167+
/** @brief The number of joints in a single JointPosition */
168+
long n_dof_;
169+
170+
/** @brief The coeff to apply to error and gradient */
171+
Eigen::VectorXd coeffs_;
172+
173+
/** @brief Bounds on the positions of each joint */
174+
std::vector<ifopt::Bounds> bounds_;
175+
176+
/** @brief Pointers to the vars used by this constraint.
177+
*
178+
* Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/
179+
std::shared_ptr<const Var> position_var_;
180+
};
108181
} // namespace trajopt_ifopt
109182
#endif

0 commit comments

Comments
 (0)