#include <joint_position_integrator.hpp>
Public Member Functions | |
| JointPositionIntegrator (ros::NodeHandle &nh) | |
| void | reset () |
| KDL::JntArray | update (const KDL::JntArray &q_dot, const KDL::JntArray &q, float dt) |
| Eigen::VectorXd | update (const Eigen::VectorXd &q_dot, const Eigen::VectorXd &q, float dt) |
| ~JointPositionIntegrator () | |
Private Member Functions | |
| bool | init () |
Private Attributes | |
| bool | got_state_ |
| float | max_joint_error_ |
| ros::NodeHandle | nh_ |
| KDL::JntArray | virt_q_ |
Implement a joint position integrator which produces joint position references given a joint velocity target.
Definition at line 13 of file joint_position_integrator.hpp.
| generic_control_toolbox::JointPositionIntegrator::JointPositionIntegrator | ( | ros::NodeHandle & | nh | ) |
Definition at line 5 of file joint_position_integrator.cpp.
|
inline |
Definition at line 17 of file joint_position_integrator.hpp.
|
private |
Get the parameters for the integrator.
Definition at line 67 of file joint_position_integrator.cpp.
| void generic_control_toolbox::JointPositionIntegrator::reset | ( | ) |
Reset the integrator's state.
Definition at line 16 of file joint_position_integrator.cpp.
| KDL::JntArray generic_control_toolbox::JointPositionIntegrator::update | ( | const KDL::JntArray & | q_dot, |
| const KDL::JntArray & | q, | ||
| float | dt | ||
| ) |
Produce an updated joint position vector for a given set of joint velocities.
The output is q = sat(q + q_dot * dt), where the function sat(.) saturates the output if any joint exceeds the configured max_joint_error.
| q_dot | The array of joint velocities to be integrated. |
| q | The current joint state. |
| dt | The integration delta. |
Definition at line 18 of file joint_position_integrator.cpp.
| Eigen::VectorXd generic_control_toolbox::JointPositionIntegrator::update | ( | const Eigen::VectorXd & | q_dot, |
| const Eigen::VectorXd & | q, | ||
| float | dt | ||
| ) |
Definition at line 43 of file joint_position_integrator.cpp.
|
private |
Definition at line 45 of file joint_position_integrator.hpp.
|
private |
Definition at line 44 of file joint_position_integrator.hpp.
|
private |
Definition at line 42 of file joint_position_integrator.hpp.
|
private |
Definition at line 43 of file joint_position_integrator.hpp.