#include <vpRobotFrankaSim.h>
|
virtual void | add_tool (const vpHomogeneousMatrix &flMe, const double mL, const vpHomogeneousMatrix &flMcom, const vpMatrix &I_L) |
|
void | get_eJe (const vpColVector &q, vpMatrix &fJe) |
|
void | get_eJe (vpMatrix &eJe_) |
|
vpHomogeneousMatrix | get_eMc () const |
|
void | get_fJe (const vpColVector &q, vpMatrix &fJe) |
|
void | get_fJe (vpMatrix &fJe) |
|
vpHomogeneousMatrix | get_flMcom () const |
|
vpHomogeneousMatrix | get_flMe () const |
|
vpHomogeneousMatrix | get_fMe () |
|
vpHomogeneousMatrix | get_fMe (const vpColVector &q) |
|
double | get_tool_mass () const |
|
void | getCoriolis (vpColVector &coriolis) |
|
void | getCoriolisMatrix (vpMatrix &coriolis) |
|
virtual void | getForceTorque (const vpRobot::vpControlFrameType frame, vpColVector &force) |
|
void | getFriction (vpColVector &friction) |
|
void | getGravity (vpColVector &gravity) |
|
void | getMass (vpMatrix &mass) |
|
virtual void | getPosition (const vpRobot::vpControlFrameType frame, vpColVector &position) |
|
virtual void | getPosition (const vpRobot::vpControlFrameType frame, vpPoseVector &position) |
|
vpRobot::vpRobotStateType | getRobotState (void) |
|
virtual void | getVelocity (const vpRobot::vpControlFrameType frame, vpColVector &d_position) |
|
virtual void | set_eMc (const vpHomogeneousMatrix &eMc) |
|
virtual void | set_flMe (const vpHomogeneousMatrix &flMe) |
|
virtual void | set_g0 (const vpColVector &g0) |
|
virtual void | setForceTorque (const vpRobot::vpControlFrameType frame, const vpColVector &force) |
|
virtual void | setPosition (const vpRobot::vpControlFrameType frame, const vpColVector &position) |
|
virtual void | setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &vel) |
|
void | setVerbose (bool verbose) |
|
| vpRobotFrankaSim () |
|
virtual | ~vpRobotFrankaSim () |
|
Franka robot simulator.
Definition at line 67 of file vpRobotFrankaSim.h.
◆ vpRobotFrankaSim()
vpRobotFrankaSim::vpRobotFrankaSim |
( |
| ) |
|
◆ ~vpRobotFrankaSim()
vpRobotFrankaSim::~vpRobotFrankaSim |
( |
| ) |
|
|
virtual |
◆ add_tool()
void vpRobotFrankaSim::add_tool |
( |
const vpHomogeneousMatrix & |
flMe, |
|
|
const double |
mL, |
|
|
const vpHomogeneousMatrix & |
flMcom, |
|
|
const vpMatrix & |
I_L |
|
) |
| |
|
virtual |
Set the tool extrinsics as the homogeneous transformation between the robot flange and the end-effector frame. It modifies the robot kinematics of the last link as well as the dynamic model due to the mass and center of mass (CoM) of the plugged tool
- Parameters
-
[in] | flMe | : Homogeneous transformation between the robot flange and end-effector frame. |
[in] | mL | : Mass of the tool attached to the end-effector. |
[in] | flMcom | : Homogeneous transformation between the robot flange and tool Center-of-Mass. |
[in] | I_L | : Tool inertia tensor in CoM frame. |
- Examples
- tutorial-franka-coppeliasim-joint-impedance-control.cpp.
Definition at line 222 of file vpRobotFrankaSim.cpp.
◆ get_eJe() [1/2]
void vpRobotFrankaSim::get_eJe |
( |
const vpColVector & |
q, |
|
|
vpMatrix & |
eJe |
|
) |
| |
Get robot Jacobian in the end-effector frame.
- Parameters
-
[in] | q | : Joint position as a 7-dim vector with values in [rad]. |
[out] | eJe | : Corresponding 6-by-7 Jacobian matrix expressed in the end-effector frame. |
Definition at line 344 of file vpRobotFrankaSim.cpp.
◆ get_eJe() [2/2]
void vpRobotFrankaSim::get_eJe |
( |
vpMatrix & |
eJe | ) |
|
Get robot Jacobian in the end-effector frame.
- Parameters
-
[out] | eJe | : 6-by-7 Jacobian matrix corresponding to the robot current joint position expressed in the end-effector frame. |
Definition at line 326 of file vpRobotFrankaSim.cpp.
◆ get_eMc()
vpHomogeneousMatrix vpRobotFrankaSim::get_eMc |
( |
| ) |
const |
◆ get_fJe() [1/2]
void vpRobotFrankaSim::get_fJe |
( |
const vpColVector & |
q, |
|
|
vpMatrix & |
fJe |
|
) |
| |
Get robot Jacobian expressed in the robot base frame.
- Parameters
-
[in] | q | : Joint position as a 7-dim vector with values in [rad]. |
[out] | fJe | : Corresponding 6-by-7 Jacobian matrix expressed in the robot base frame. |
Definition at line 291 of file vpRobotFrankaSim.cpp.
◆ get_fJe() [2/2]
void vpRobotFrankaSim::get_fJe |
( |
vpMatrix & |
fJe | ) |
|
◆ get_flMcom()
vpHomogeneousMatrix vpRobotFrankaSim::get_flMcom |
( |
| ) |
const |
◆ get_flMe()
vpHomogeneousMatrix vpRobotFrankaSim::get_flMe |
( |
| ) |
const |
◆ get_fMe() [1/2]
vpHomogeneousMatrix vpRobotFrankaSim::get_fMe |
( |
| ) |
|
Get robot direct kinematics corresponding to the homogeneous transformation between the robot base frame and the end-effector for the current joint position.
- Returns
- Homogeneous transformation between the robot base frame and the end-effector.
Definition at line 942 of file vpRobotFrankaSim.cpp.
◆ get_fMe() [2/2]
vpHomogeneousMatrix vpRobotFrankaSim::get_fMe |
( |
const vpColVector & |
q | ) |
|
◆ get_tool_mass()
double vpRobotFrankaSim::get_tool_mass |
( |
| ) |
const |
◆ getCoriolis()
void vpRobotFrankaSim::getCoriolis |
( |
vpColVector & |
coriolis | ) |
|
◆ getCoriolisMatrix()
void vpRobotFrankaSim::getCoriolisMatrix |
( |
vpMatrix & |
coriolis | ) |
|
Get the Coriolis matrix (state-space equation) calculated from the current robot state: , in .
- Parameters
-
[out] | coriolis | : 7x7 Coriolis matrix, row-major. |
Definition at line 1056 of file vpRobotFrankaSim.cpp.
◆ getForceTorque()
void vpRobotFrankaSim::getForceTorque |
( |
const vpRobot::vpControlFrameType |
frame, |
|
|
vpColVector & |
force |
|
) |
| |
|
virtual |
Get robot force/torque.
- Parameters
-
[in] | frame | : Control frame to consider. |
[out] | force | : Measured force/torque.
- When
frame is set to vpRobot::JOINT_STATE, force is a 7-dim vector that contains torques in [Nm].
- When
frame is set to vpRobot::REFERENCE_FRAME, force is a 6-dim vector that contains cartesian end-effector forces/torques expressed in the reference frame. This vector contains 3 forces [Fx, Fy, Fz] in [N] followed by 3 torques [Tx, Ty, Tz] in [Nm].
- When
frame is set to vpRobot::END_EFFECTOR_FRAME, force is a 6-dim vector that contains cartesian end-effector forces/torques expressed in the end-effector frame. This vector contains 3 forces [Fx, Fy, Fz] in [N] followed by 3 torques [Tx, Ty, Tz] in [Nm].
|
- Examples
- tutorial-franka-coppeliasim-cartesian-impedance-control.cpp, and tutorial-franka-coppeliasim-joint-impedance-control.cpp.
Definition at line 825 of file vpRobotFrankaSim.cpp.
◆ getFriction()
void vpRobotFrankaSim::getFriction |
( |
vpColVector & |
friction | ) |
|
◆ getGravity()
void vpRobotFrankaSim::getGravity |
( |
vpColVector & |
gravity | ) |
|
Get the gravity vector calculated from the current robot state. Unit: .
- Parameters
-
[out] | gravity | : Gravity 7-dim vector. |
Definition at line 1030 of file vpRobotFrankaSim.cpp.
◆ getMass()
void vpRobotFrankaSim::getMass |
( |
vpMatrix & |
mass | ) |
|
◆ getPosition() [1/2]
void vpRobotFrankaSim::getPosition |
( |
const vpRobot::vpControlFrameType |
frame, |
|
|
vpColVector & |
position |
|
) |
| |
|
virtual |
Get robot position.
- Parameters
-
[in] | frame | : Control frame to consider. |
[out] | position | : Robot position.
- When frame is set to vpRobot::JOINT_STATE,
position is a 7-dim vector that contains the joint positions in [rad].
- When
frame is set to vpRobot::END_EFFECTOR_FRAME, position is a 6-dim vector that contains the pose of the end-effector in the robot base frame. This pose vector contains the 3 translations values [tx, ty, tz] followed by the 3 rotations using the axis-angle representation [tux, tuy, tyz] with values in [rad].
- When
frame is set to vpRobot::CAMERA_FRAME, position is a 6-dim vector that contains the pose of the camera in the robot base frame. This pose vector contains the 3 translations values [tx, ty, tz] followed by the 3 rotations using the axis-angle representation [tux, tuy, tyz] with values in [rad].
|
- Examples
- test-vel.cpp, tutorial-franka-coppeliasim-cartesian-impedance-control.cpp, tutorial-franka-coppeliasim-dual-arm.cpp, tutorial-franka-coppeliasim-ibvs-apriltag.cpp, tutorial-franka-coppeliasim-joint-impedance-control.cpp, and tutorial-franka-coppeliasim-pbvs-apriltag.cpp.
Definition at line 417 of file vpRobotFrankaSim.cpp.
◆ getPosition() [2/2]
void vpRobotFrankaSim::getPosition |
( |
const vpRobot::vpControlFrameType |
frame, |
|
|
vpPoseVector & |
position |
|
) |
| |
|
virtual |
Get robot cartesian position.
- Parameters
-
[in] | frame | : Control frame to consider. Admissible values are vpRobot::END_EFFECTOR_FRAME and vpRobot::CAMERA_FRAME. |
[out] | position | : Robot cartesian position.
- When
frame is set to vpRobot::END_EFFECTOR_FRAME, position is a 6-dim pose vector that contains the pose of the end-effector in the robot base frame. This pose vector contains the 3 translations values [tx, ty, tz] followed by the 3 rotations using the axis-angle representation [tux, tuy, tyz] with values in [rad].
- When
frame is set to vpRobot::CAMERA_FRAME, position is a 6-dim pose vector that contains the pose of the camera in the robot base frame. This pose vector contains the 3 translations values [tx, ty, tz] followed by the 3 rotations using the axis-angle representation [tux, tuy, tyz] with values in [rad].
|
Definition at line 472 of file vpRobotFrankaSim.cpp.
◆ getRobotState()
vpRobot::vpRobotStateType vpRobotFrankaSim::getRobotState |
( |
void |
| ) |
|
◆ getVelDes()
vpColVector vpRobotFrankaSim::getVelDes |
( |
| ) |
|
|
protected |
◆ getVelocity()
void vpRobotFrankaSim::getVelocity |
( |
const vpRobot::vpControlFrameType |
frame, |
|
|
vpColVector & |
velocity |
|
) |
| |
|
virtual |
Get robot velocity.
- Parameters
-
[in] | frame | : Control frame to consider. |
[out] | velocity | : Robot velocity.
- When
frame is set to vpRobot::JOINT_STATE, velocity is a 7-dim vector corresponding to the joint velocities in [rad/s].
- When
frame is set to vpRobot::END_EFFECTOR_FRAME, velocity is a 6-dim vector corresponding to the cartesian velocities of the end-effector expressed in the end-effector. This vector contains the 3 translational velocities in [m/s] followed by the 3 rotational velocities in [rad/s].
- When
frame is set to vpRobot::REFERENCE_FRAME, velocity is a 6-dim vector corresponding to the cartesian velocities of the end-effector expressed in the robot base frame. This vector contains the 3 translational velocities in [m/s] followed by the 3 rotational velocities in [rad/s].
|
- Examples
- tutorial-franka-coppeliasim-cartesian-impedance-control.cpp, tutorial-franka-coppeliasim-dual-arm.cpp, and tutorial-franka-coppeliasim-joint-impedance-control.cpp.
Definition at line 627 of file vpRobotFrankaSim.cpp.
◆ set_eMc()
void vpRobotFrankaSim::set_eMc |
( |
const vpHomogeneousMatrix & |
eMc | ) |
|
|
virtual |
◆ set_flMe()
void vpRobotFrankaSim::set_flMe |
( |
const vpHomogeneousMatrix & |
flMe | ) |
|
|
virtual |
Set end-effecto extrinsics as the homogeneous transformation between the flange and the end-effector frame.
- Parameters
-
[in] | flMe | : Homogeneous transformation between the end-effector and the flange frame. |
Definition at line 172 of file vpRobotFrankaSim.cpp.
◆ set_g0()
void vpRobotFrankaSim::set_g0 |
( |
const vpColVector & |
g0 | ) |
|
|
virtual |
Set the absolute acceleration vector in robot's base frame.
- Parameters
-
[in] | g0 | : gravitational acceleration vector in base frame. |
Definition at line 205 of file vpRobotFrankaSim.cpp.
◆ setForceTorque()
void vpRobotFrankaSim::setForceTorque |
( |
const vpRobot::vpControlFrameType |
frame, |
|
|
const vpColVector & |
force |
|
) |
| |
|
virtual |
Set robot force/torque.
- Parameters
-
[in] | frame | : Control frame to consider. |
[in] | force | : Force/torque applied to the robot.
- When
frame is set to vpRobot::JOINT_STATE, force is a 7-dim vector that contains torques in [Nm].
- When
frame is set to vpRobot::REFERENCE_FRAME, force is a 6-dim vector that contains cartesian end-effector forces/torques expressed in the reference frame. This vector contains 3 forces [Fx, Fy, Fz] in [N] followed by 3 torques [Tx, Ty, Tz] in [Nm].
- When
frame is set to vpRobot::END_EFFECTOR_FRAME, force is a 6-dim vector that contains cartesian end-effector forces/torques expressed in the end-effector frame. This vector contains 3 forces [Fx, Fy, Fz] in [N] followed by 3 torques [Tx, Ty, Tz] in [Nm].
|
- Examples
- tutorial-franka-coppeliasim-cartesian-impedance-control.cpp, tutorial-franka-coppeliasim-dual-arm.cpp, and tutorial-franka-coppeliasim-joint-impedance-control.cpp.
Definition at line 875 of file vpRobotFrankaSim.cpp.
◆ setPosition()
void vpRobotFrankaSim::setPosition |
( |
const vpRobot::vpControlFrameType |
frame, |
|
|
const vpColVector & |
position |
|
) |
| |
|
virtual |
Set robot position.
- Parameters
-
[in] | frame | : Control frame to consider. |
[in] | position | : Position to reach.
- When
frame is set to vpRobot::JOINT_STATE, position is a 7-dim vector that contains joint positions in [rad].
- When
frame is set to vpRobot::END_EFFECTOR_FRAME, position is a 6-dim vector containing the pose of the end-effector in the robot base frame. This pose contains the 3 translations values [tx, ty, tz] followed by the 3 rotations using the axis-angle representation [tux, tuy, tyz] with values in [rad].
- When
frame is set to vpRobot::CAMERA_FRAME, position is a 6-dim vector containing the pose of the camera in the robot base frame. This pose contains the 3 translations values [tx, ty, tz] followed by the 3 rotations using the axis-angle representation [tux, tuy, tyz] with values in [rad].
|
Reimplemented in vpROSRobotFrankaCoppeliasim.
Definition at line 505 of file vpRobotFrankaSim.cpp.
◆ setVelocity()
void vpRobotFrankaSim::setVelocity |
( |
const vpRobot::vpControlFrameType |
frame, |
|
|
const vpColVector & |
velocity |
|
) |
| |
|
virtual |
Set robot velocity.
- Parameters
-
[in] | frame | : Control frame to consider. |
[in] | velocity | : Velocity to apply to the robot.
- When
frame is set to vpRobot::JOINT_STATE, velocity is a 7-dim vector corresponding to the joint velocities in [rad/s].
- When
frame is set to vpRobot::REFERENCE_FRAME, velocity is a 6-dim vector corresponding to the cartesian velocities of the end-effector expressed in the robot base frame. This vector contains the 3 translational velocities in [m/s] followed by the 3 rotational velocities in [rad/s].
- When
frame is set to vpRobot::END_EFFECTOR_FRAME, velocity is a 6-dim vector corresponding to the cartesian velocities of the end-effector expressed in the end-effector frame. This vector contains the 3 translational velocities in [m/s] followed by the 3 rotational velocities in [rad/s].
- When
frame is set to vpRobot::CAMERA_FRAME, velocity is a 6-dim vector corresponding to the cartesian velocities of the camera expressed in the camera frame. This vector contains the 3 translational velocities in [m/s] followed by the 3 rotational velocities in [rad/s].
|
- Examples
- test-vel.cpp, tutorial-franka-coppeliasim-dual-arm.cpp, tutorial-franka-coppeliasim-ibvs-apriltag.cpp, and tutorial-franka-coppeliasim-pbvs-apriltag.cpp.
Definition at line 683 of file vpRobotFrankaSim.cpp.
◆ setVerbose()
void vpRobotFrankaSim::setVerbose |
( |
bool |
verbose | ) |
|
|
inline |
◆ solveIK()
vpColVector vpRobotFrankaSim::solveIK |
( |
const vpHomogeneousMatrix & |
wMe | ) |
|
|
protected |
Solve inverse kinematics.
- Parameters
-
wMe | : Robot base frame to end-effector homogeneous transformation. |
- Returns
- Corresponding joint position as a 7-dim vector with values in [rad].
Definition at line 566 of file vpRobotFrankaSim.cpp.
◆ m_camMounted
bool vpRobotFrankaSim::m_camMounted |
|
protected |
◆ m_chain_kdl
KDL::Chain vpRobotFrankaSim::m_chain_kdl |
|
protected |
◆ m_diffIkSolver_kdl
KDL::ChainIkSolverVel_pinv* vpRobotFrankaSim::m_diffIkSolver_kdl |
|
protected |
◆ m_dq
vpColVector vpRobotFrankaSim::m_dq |
|
protected |
◆ m_dq_des
vpColVector vpRobotFrankaSim::m_dq_des |
|
protected |
◆ m_dq_des_filt
vpColVector vpRobotFrankaSim::m_dq_des_filt |
|
protected |
◆ m_dq_des_kdl
KDL::JntArray vpRobotFrankaSim::m_dq_des_kdl |
|
protected |
◆ m_eMc
vpHomogeneousMatrix vpRobotFrankaSim::m_eMc |
|
protected |
◆ m_eVc
vpVelocityTwistMatrix vpRobotFrankaSim::m_eVc |
|
protected |
◆ m_fksolver_kdl
KDL::ChainFkSolverPos_recursive* vpRobotFrankaSim::m_fksolver_kdl |
|
protected |
◆ m_flMcom
vpHomogeneousMatrix vpRobotFrankaSim::m_flMcom |
|
protected |
◆ m_flMe
vpHomogeneousMatrix vpRobotFrankaSim::m_flMe |
|
protected |
◆ m_g0
vpColVector vpRobotFrankaSim::m_g0 |
|
protected |
◆ m_iksolver_JL_kdl
KDL::ChainIkSolverPos_NR_JL* vpRobotFrankaSim::m_iksolver_JL_kdl |
|
protected |
◆ m_Il
vpMatrix vpRobotFrankaSim::m_Il |
|
protected |
◆ m_jacobianSolver_kdl
KDL::ChainJntToJacSolver* vpRobotFrankaSim::m_jacobianSolver_kdl |
|
protected |
◆ m_mL
double vpRobotFrankaSim::m_mL |
|
protected |
◆ m_mutex
std::mutex vpRobotFrankaSim::m_mutex |
|
protected |
◆ m_q
vpColVector vpRobotFrankaSim::m_q |
|
protected |
◆ m_q_des
vpColVector vpRobotFrankaSim::m_q_des |
|
protected |
◆ m_q_kdl
KDL::JntArray vpRobotFrankaSim::m_q_kdl |
|
protected |
◆ m_q_max_kdl
KDL::JntArray vpRobotFrankaSim::m_q_max_kdl |
|
protected |
◆ m_q_min_kdl
KDL::JntArray vpRobotFrankaSim::m_q_min_kdl |
|
protected |
◆ m_stateRobot
vpRobot::vpRobotStateType vpRobotFrankaSim::m_stateRobot |
|
protected |
◆ m_tau_J
vpColVector vpRobotFrankaSim::m_tau_J |
|
protected |
◆ m_tau_J_des
vpColVector vpRobotFrankaSim::m_tau_J_des |
|
protected |
◆ m_tau_J_des_filt
vpColVector vpRobotFrankaSim::m_tau_J_des_filt |
|
protected |
◆ m_toolMounted
bool vpRobotFrankaSim::m_toolMounted |
|
protected |
◆ m_v_cart_des
vpColVector vpRobotFrankaSim::m_v_cart_des |
|
protected |
◆ m_verbose
bool vpRobotFrankaSim::m_verbose |
|
protected |
The documentation for this class was generated from the following files:
visp_ros
Author(s): Francois Pasteau, Fabien Spindler, Gatien Gaumerais, Alexander Oliva
autogenerated on Wed Mar 2 2022 01:13:33