40 params.dt_constraint_range_of_motion_,
41 "RangeOfMotionBox-" + std::to_string(ee))
57 return node*
k3D + dim;
67 Vector3d vector_base_to_ee_W = pos_ee_W - base_W;
68 Vector3d vector_base_to_ee_B = b_R_w*(vector_base_to_ee_W);
70 g.middleRows(
GetRow(k,
X),
k3D) = vector_base_to_ee_B;
76 for (
int dim=0; dim<
k3D; ++dim) {
81 bounds.at(
GetRow(k,dim)) = b;
109 jac.middleRows(row_start,
k3D) = b_R_w*
ee_motion_->GetJacobianOfPosWrtDurations(t);
Constraints evaluated at discretized times along a trajectory.
NodeSpline::Ptr ee_motion_
the linear position of the endeffectors.
std::shared_ptr< KinematicModel > Ptr
static std::string EESchedule(uint ee)
Eigen::SparseMatrix< double, Eigen::RowMajor > MatrixSXd
int GetRow(int node, int dimension) const
Holds pointers to fully constructed splines, that are linked to the optimization variables.
Eigen::Vector3d nominal_ee_pos_B_
int GetNumberOfNodes() const
Converts Euler angles and derivatives to angular quantities.
NodeSpline::Ptr base_linear_
Eigen::Vector3d max_deviation_from_nominal_
RangeOfMotionConstraint(const KinematicModel::Ptr &robot_model, const Parameters ¶ms, const EE &ee, const SplineHolder &spline_holder)
Constructs a constraint instance.
static std::string EEMotionNodes(uint ee)
NodeSpline::Ptr base_angular_
EulerConverter base_angular_
the orientation of the base.
Holds the parameters to tune the optimization problem.
MatrixSXd GetRotationMatrixBaseToWorld(double t) const
Converts the Euler angles at time t to a rotation matrix.
void UpdateConstraintAtInstance(double t, int k, VectorXd &g) const override
Sets the constraint value a specific time t, corresponding to node k.
static const std::string base_ang_nodes
NodeSpline::Ptr base_linear_
the linear position of the base.
Jacobian DerivOfRotVecMult(double t, const Vector3d &v, bool inverse) const
Returns the derivative of result of the linear equation M*v.
void UpdateBoundsAtInstance(double t, int k, VecBound &) const override
Sets upper/lower bound a specific time t, corresponding to node k.
virtual void UpdateJacobianAtInstance(double t, int k, std::string, Jacobian &) const override
Sets Jacobian rows at a specific time t, corresponding to node k.
std::vector< NodeSpline::Ptr > ee_motion_
static const std::string base_lin_nodes