15 #include <dynamic-graph/factory.h> 22 ClampWorkspace::ClampWorkspace(
const string &fName)
25 NULL,
"ClampWorkspace(" +
name +
")::input(double)::positionref"),
26 positionSIN(NULL,
"ClampWorkspace(" +
name +
")::input(double)::position")
30 positionrefSIN << positionSIN,
31 "ClampWorkspace(" +
name +
")::output(vector)::alpha")
35 positionrefSIN << positionSIN,
36 "ClampWorkspace(" +
name +
")::output(vector)::alphabar")
40 positionrefSIN << positionSIN,
41 "ClampWorkspace(" +
name +
")::output(vector)::ref")
60 theta_min(-30. * 3.14159 / 180.),
61 theta_max(5. * 3.14159 / 180.),
68 bounds[0] = std::make_pair(0.15, 0.5);
69 bounds[1] = std::make_pair(-0.4, -0.25);
70 bounds[2] = std::make_pair(0.15, 0.55);
91 for (
int i = 0;
i < 3; ++
i) {
92 double check_min = std::max(
x(
i) -
bounds[
i].first, 0.);
93 double check_max = std::max(
bounds[
i].second -
x(
i), 0.);
94 double dm = std::min(check_min, check_max);
115 alpha(
i,
i) = 0.5 * (1 + tanh(1 / Y - 1 / (1 - Y)));
128 alpha(
i + 3,
i + 3) = 0.5 * (1 + tanh(1 / Y - 1 / (1 - Y)));
137 for (
int i = 0;
i < 3; ++
i) {
139 prefMp_tmp(
i, 3) = 0;
148 tmp =
alpha * prefTp;
149 alpha = pTpref * tmp;
155 for (
int i = 0;
i < 3; ++
i) {
161 rpy(1) = -3.14159256 / 2;
165 Eigen::Affine3d _Rd(Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) *
166 Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) *
167 Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()));
170 Eigen::Affine3d _tmpaffine;
171 _tmpaffine = Eigen::Translation3d(
pd);
197 os <<
"ClampWorkspace<" <<
name <<
">" << endl << endl;
198 os <<
"alpha: " <<
alpha << endl;
199 os <<
"pos in ws: " <<
prefMp << endl;
200 os <<
"bounds: " <<
bounds[0].first <<
" " <<
bounds[0].second <<
" " 202 <<
" " <<
bounds[2].second << endl;
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
enum dynamicgraph::sot::ClampWorkspace::@0 frame
dynamicgraph::Matrix alphabar
void signalRegistration(const SignalArray< int > &signals)
dynamicgraph::SignalTimeDependent< dynamicgraph::Matrix, int > alphaSOUT
dynamicgraph::SignalTimeDependent< MatrixHomogeneous, int > handrefSOUT
dynamicgraph::SignalPtr< MatrixHomogeneous, int > positionrefSIN
void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ClampWorkspace, "ClampWorkspace")
virtual void display(std::ostream &) const
Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw
MatrixHomogeneous handref
virtual const T & access(const Time &t)
std::pair< double, double > bounds[3]
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixTwist
dynamicgraph::SignalTimeDependent< dynamicgraph::Matrix, int > alphabarSOUT
virtual MatrixHomogeneous & computeRef(MatrixHomogeneous &res, int time)
virtual dynamicgraph::Matrix & computeOutputBar(dynamicgraph::Matrix &res, int time)
dynamicgraph::Matrix alpha
dynamicgraph::SignalPtr< MatrixHomogeneous, int > positionSIN
virtual dynamicgraph::Matrix & computeOutput(dynamicgraph::Matrix &res, int time)