Go to the documentation of this file.
17 sotJTE__INIT sotJTE_initiator;
18 #endif // #ifdef VP_DEBUG
22 #include <dynamic-graph/factory.h>
34 "SotJointTrajectoryEntity");
36 SotJointTrajectoryEntity::SotJointTrajectoryEntity(
const std::string &
n)
38 refresherSINTERN(
"SotJointTrajectoryEntity(" +
n +
39 ")::intern(dummy)::refresher"),
42 refresherSINTERN << trajectorySIN,
43 "SotJointTrajectory(" +
n +
")::onestepofupdate"),
47 "SotJointTrajectory(" +
n +
")::output(vector)::position"),
50 "SotJointTrajectory(" +
n +
")::output(vector)::com"),
53 "SotJointTrajectory(" +
n +
")::output(vector)::zmp"),
57 "SotJointTrajectory(" +
n +
")::output(MatrixHomogeneous)::waist"),
60 "SotJointTrajectory(" +
n +
")::output(uint)::seqid"),
61 trajectorySIN(NULL,
"SotJointTrajectory(" +
n +
62 ")::input(trajectory)::trajectoryIN"),
64 traj_timestamp_(0, 0),
67 using namespace command;
75 std::string docstring;
78 " initialize the first trajectory.\n"
86 "string (trajectory)")));
94 if (possize == 0)
return;
106 i < possize - 2;
i++, j++)
113 waistXYZTheta.resize(4);
115 waistXYZTheta(0) =
com_(0);
116 waistXYZTheta(1) =
com_(1);
117 waistXYZTheta(2) = 0.65;
118 waistXYZTheta(3) =
com_(2);
125 i < possize;
i++, j++)
135 <<
" aTrajectory.header_.stamp_" << aTrajectory.
header_.
stamp_;
143 sotDEBUG(3) <<
"index: " <<
index_ <<
" aTrajectory.points_.size(): "
161 sotDEBUG(3) <<
"step 1 " << std::endl
162 <<
"header: " << std::endl
170 <<
" aTrajectory.points_.size():" << aTrajectory.
points_.size()
184 sotDEBUG(3) <<
"current_traj_.points_.size()="
185 <<
deque_traj_.front().points_.size() << std::endl;
195 sotDEBUG(3) <<
"index_=current_traj_.points_.size()-1;" << std::endl;
198 sotDEBUG(3) <<
"index_:" <<
index_ <<
" current_traj_.points_.size():"
199 <<
deque_traj_.front().points_.size() << std::endl;
215 sotDEBUG(4) <<
"Overwrite trajectory without completion." <<
index_ <<
" "
216 <<
deque_traj_.front().points_.size() << std::endl;
219 sotDEBUG(4) <<
"Finished to read trajectorySIN" << std::endl;
222 sotDEBUG(4) <<
"Finished to update trajectory" << std::endl;
230 assert(xyztheta.size() == 4);
235 Eigen::Affine3d trans;
236 trans = Eigen::Translation3d(
t);
237 Eigen::Affine3d _Rd(Eigen::AngleAxisd(xyztheta(3), Eigen::Vector3d::UnitZ()));
303 std::istringstream is(as);
virtual void display(std::ostream &os) const
dynamicgraph::SignalTimeDependent< sot::MatrixHomogeneous, sigtime_t > waistSOUT
Publish waist for each evaluation of the graph.
size_type deserialize(std::istringstream &is)
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
dynamicgraph::Vector & getNextPosition(dynamicgraph::Vector &pos, const sigtime_t &time)
Return the next pose for the legs.
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, sigtime_t > zmpSOUT
Publish zmp for each evaluation of the graph.
dynamicgraph::SignalTimeDependent< size_type, sigtime_t > refresherSINTERN
This object handles trajectory of quantities and publish them as signals.
size_type & OneStepOfUpdate(size_type &dummy, const sigtime_t &time)
Perform one update of the signals.
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, sigtime_t > positionSOUT
Publish pose for each evaluation of the graph.
dynamicgraph::size_type size_type
sot::Trajectory init_traj_
Initial state of the trajectory.
std::vector< double > positions_
std::deque< sot::Trajectory > deque_traj_
Queue of trajectories.
#define sotDEBUGOUT(level)
std::deque< sot::Trajectory >::size_type index_
Index on the point along the trajectory.
void UpdateTrajectory(const Trajectory &aTrajectory)
Update the entity with the trajectory aTrajectory.
std::string docCommandVoid1(const std::string &doc, const std::string &type)
void UpdatePoint(const JointTrajectoryPoint &aJTP)
Update the entity with the current point of the trajectory.
sot::MatrixHomogeneous XYZThetaToMatrixHomogeneous(const dynamicgraph::Vector &xyztheta)
Convert a xyztheta vector into an homogeneous matrix.
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, sigtime_t > comSOUT
Publish com for each evaluation of the graph.
void loadFile(const std::string &name)
#define sotDEBUGIN(level)
dynamicgraph::Vector & getNextCoM(dynamicgraph::Vector &com, const sigtime_t &time)
Return the next com.
dynamicgraph::Vector com_
Store the center of mass.
sot::MatrixHomogeneous & getNextWaist(sot::MatrixHomogeneous &waist, const sigtime_t &time)
Return the next waist.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SotJointTrajectoryEntity, "SotJointTrajectoryEntity")
timestamp traj_timestamp_
Keep the starting time as an identifier of the trajector.
std::size_t seqid_
Store the current seq identifier.
sot::MatrixHomogeneous waist_
Store the waist position.
void setInitTraj(const std::string &os)
Implements the parsing and the affectation of initial trajectory.
std::vector< JointTrajectoryPoint > points_
dynamicgraph::Vector & getNextCoP(dynamicgraph::Vector &cop, const sigtime_t &time)
Return the next cop.
void addCommand(const std::string &name, command::Command *command)
dynamicgraph::Vector pose_
Store the pos;.
dynamicgraph::SignalPtr< Trajectory, sigtime_t > trajectorySIN
Read a trajectory.
static void openFile(const char *filename=DEBUG_FILENAME_DEFAULT)
std::size_t & getSeqId(std::size_t &seqid, const sigtime_t &time)
Return the current seq identified of the current trajectory.
void signalRegistration(const SignalArray< sigtime_t > &signals)
SignalTimeDependent< Dummy, sigtime_t > OneStepOfUpdateS
Internal signal to trigger one step of the algorithm.
dynamicgraph::Vector cop_
Store the center of pressure ZMP.
CommandVoid1< E, T > * makeCommandVoid1(E &entity, boost::function< void(const T &)> function, const std::string &docString)
dynamicgraph::SignalTimeDependent< std::size_t, sigtime_t > seqIdSOUT
Publish ID of the trajectory currently realized.
sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Tue Oct 24 2023 02:26:31