Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <towr/constraints/range_of_motion_constraint.h>
00031 #include <towr/variables/variable_names.h>
00032
00033 namespace towr {
00034
00035 RangeOfMotionConstraint::RangeOfMotionConstraint (const KinematicModel::Ptr& model,
00036 const Parameters& params,
00037 const EE& ee,
00038 const SplineHolder& spline_holder)
00039 :TimeDiscretizationConstraint(params.t_total_,
00040 params.dt_constraint_range_of_motion_,
00041 "RangeOfMotionBox-" + std::to_string(ee))
00042 {
00043 base_linear_ = spline_holder.base_linear_;
00044 base_angular_ = EulerConverter(spline_holder.base_angular_);
00045 ee_motion_ = spline_holder.ee_motion_.at(ee);
00046
00047 max_deviation_from_nominal_ = model->GetMaximumDeviationFromNominal();
00048 nominal_ee_pos_B_ = model->GetNominalStanceInBase().at(ee);
00049 ee_ = ee;
00050
00051 SetRows(GetNumberOfNodes()*k3D);
00052 }
00053
00054 int
00055 RangeOfMotionConstraint::GetRow (int node, int dim) const
00056 {
00057 return node*k3D + dim;
00058 }
00059
00060 void
00061 RangeOfMotionConstraint::UpdateConstraintAtInstance (double t, int k, VectorXd& g) const
00062 {
00063 Vector3d base_W = base_linear_->GetPoint(t).p();
00064 Vector3d pos_ee_W = ee_motion_->GetPoint(t).p();
00065 EulerConverter::MatrixSXd b_R_w = base_angular_.GetRotationMatrixBaseToWorld(t).transpose();
00066
00067 Vector3d vector_base_to_ee_W = pos_ee_W - base_W;
00068 Vector3d vector_base_to_ee_B = b_R_w*(vector_base_to_ee_W);
00069
00070 g.middleRows(GetRow(k, X), k3D) = vector_base_to_ee_B;
00071 }
00072
00073 void
00074 RangeOfMotionConstraint::UpdateBoundsAtInstance (double t, int k, VecBound& bounds) const
00075 {
00076 for (int dim=0; dim<k3D; ++dim) {
00077 ifopt::Bounds b;
00078 b += nominal_ee_pos_B_(dim);
00079 b.upper_ += max_deviation_from_nominal_(dim);
00080 b.lower_ -= max_deviation_from_nominal_(dim);
00081 bounds.at(GetRow(k,dim)) = b;
00082 }
00083 }
00084
00085 void
00086 RangeOfMotionConstraint::UpdateJacobianAtInstance (double t, int k,
00087 std::string var_set,
00088 Jacobian& jac) const
00089 {
00090 EulerConverter::MatrixSXd b_R_w = base_angular_.GetRotationMatrixBaseToWorld(t).transpose();
00091 int row_start = GetRow(k,X);
00092
00093 if (var_set == id::base_lin_nodes) {
00094 jac.middleRows(row_start, k3D) = -1*b_R_w*base_linear_->GetJacobianWrtNodes(t, kPos);
00095 }
00096
00097 if (var_set == id::base_ang_nodes) {
00098 Vector3d base_W = base_linear_->GetPoint(t).p();
00099 Vector3d ee_pos_W = ee_motion_->GetPoint(t).p();
00100 Vector3d r_W = ee_pos_W - base_W;
00101 jac.middleRows(row_start, k3D) = base_angular_.DerivOfRotVecMult(t,r_W, true);
00102 }
00103
00104 if (var_set == id::EEMotionNodes(ee_)) {
00105 jac.middleRows(row_start, k3D) = b_R_w*ee_motion_->GetJacobianWrtNodes(t,kPos);
00106 }
00107
00108 if (var_set == id::EESchedule(ee_)) {
00109 jac.middleRows(row_start, k3D) = b_R_w*ee_motion_->GetJacobianOfPosWrtDurations(t);
00110 }
00111
00112 }
00113
00114 }
00115