Implement joint trajectory player #169
Conversation
…nts of the trajectory file added. Initial trajectory to go to the initial desired position added. Removed iDynTree vectors. Some problems fixed.
…deleted from joint-trajectory-player code.
isorrentino
changed the title
| if (!parametersHandler->getParameter("trajectory_file", trajectoryFile)) | ||
| return false; |
There was a problem hiding this comment.
I would print an error here
| Eigen::VectorXd m_currentJointPos; | ||
|
|
||
| std::shared_ptr<yarp::dev::PolyDriver> m_robotDevice; | ||
|
|
||
| RobotInterface::YarpRobotControl m_robotControl; | ||
| RobotInterface::YarpSensorBridge m_sensorBridge; |
There was a problem hiding this comment.
Could you please add the documentation here?
|
|
||
| #include <yarp/dev/IEncoders.h> | ||
|
|
||
| using Vector1d = Eigen::Matrix<double, 1, 1>; |
There was a problem hiding this comment.
I think you do not need this
| m_numOfJoints = 0; | ||
| yarp::dev::IEncoders* axis; | ||
| m_robotDevice->view(axis); | ||
| if (axis != nullptr) | ||
| axis->getAxes(&m_numOfJoints); |
There was a problem hiding this comment.
Is there another way to get this information in the RobotInterface?
| } | ||
|
|
||
| std::pair<bool, std::deque<Eigen::VectorXd>> | ||
| Module::readStateFromFile(const std::string& filename, const std::size_t num_fields) |
| m_robotControl.checkMotionDone(isMotionDone, isMimeExpired, jointlist); | ||
|
|
||
| if (isMotionDone) |
There was a problem hiding this comment.
This function was not meant to be used in this way. What I did in the past is to create a state machine and use checkMotionDone as a trigger to change the state. We can discuss this offline
| fileName << "Dataset_Measured_" << m_robotControl.getJointList().front() << "_" | ||
| << std::put_time(&tm, "%Y_%m_%d_%H_%M_%S") << ".txt"; | ||
| stream.open(fileName.str().c_str()); |
There was a problem hiding this comment.
If I'm not mistaken the main purpose of this application is to collect some data to analyze them in matlab. In this case you may also use matio-cpp developed by @S-Dafarra. You can find a usage example in #62
There was a problem hiding this comment.
Standalone short Example usage: for reading and writing.
|
I modified the code based on @GiulioRomualdi suggestions. The PR is again ready for review. |
The application reads a trajectory from a file and executes it either on a simulated robot or on a real robot. It also saves the measured joint positions. It also allows you to specify in a configuration file which robot joints to use.