Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
vpROSRobotFrankaCoppeliasim Class Reference

#include <vpROSRobotFrankaCoppeliasim.h>

Inheritance diagram for vpROSRobotFrankaCoppeliasim:
Inheritance graph
[legend]

Public Member Functions

void connect (const std::string &robot_ID="")
 
void coppeliasimPauseSimulation (double sleep_ms=1000.)
 
void coppeliasimStartSimulation (double sleep_ms=1000.)
 
void coppeliasimStopSimulation (double sleep_ms=1000.)
 
void coppeliasimTriggerNextStep ()
 
int getCoppeliasimSimulationState ()
 
bool getCoppeliasimSimulationStepDone ()
 
double getCoppeliasimSimulationTime ()
 
bool getCoppeliasimSyncMode ()
 
bool isConnected () const
 
void setCoppeliasimSimulationStepDone (bool simulationStepDone)
 
void setCoppeliasimSyncMode (bool enable, double sleep_ms=1000.)
 
void setPosition (const vpRobot::vpControlFrameType frame, const vpColVector &position)
 
vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState)
 
void setTopic_eMc (const std::string &topic_eMc)
 
void setTopic_flMcom (const std::string &topic_flMcom)
 
void setTopic_flMe (const std::string &topic_flMe)
 
void setTopic_g0 (const std::string &topic_g0)
 
void setTopic_toolInertia (const std::string &topic_toolInertia)
 
void setTopicJointState (const std::string &topic_jointState)
 
void setTopicJointStateCmd (const std::string &topic_jointStateCmd)
 
void setTopicRobotStateCmd (const std::string &topic_robotState)
 
 vpROSRobotFrankaCoppeliasim ()
 
void wait (double timestamp_second, double duration_second)
 
virtual ~vpROSRobotFrankaCoppeliasim ()
 
- Public Member Functions inherited from vpRobotFrankaSim
virtual void add_tool (const vpHomogeneousMatrix &flMe, const double mL, const vpHomogeneousMatrix &flMcom, const vpMatrix &I_L)
 
void get_eJe (vpMatrix &eJe_)
 
void get_eJe (const vpColVector &q, vpMatrix &fJe)
 
vpHomogeneousMatrix get_eMc () const
 
void get_fJe (vpMatrix &fJe)
 
void get_fJe (const vpColVector &q, vpMatrix &fJe)
 
vpHomogeneousMatrix get_flMcom () const
 
vpHomogeneousMatrix get_flMe () const
 
vpHomogeneousMatrix get_fMe (const vpColVector &q)
 
vpHomogeneousMatrix get_fMe ()
 
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 setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &vel)
 
void setVerbose (bool verbose)
 
 vpRobotFrankaSim ()
 
virtual ~vpRobotFrankaSim ()
 

Protected Member Functions

void callback_eMc (const geometry_msgs::Pose &pose_msg)
 
void callback_flMe (const geometry_msgs::Pose &pose_msg)
 
void callback_g0 (const geometry_msgs::Vector3 &g0_msg)
 
void callback_toolInertia (const geometry_msgs::Inertia &inertia_msg)
 
void callbackJointState (const sensor_msgs::JointState &joint_state)
 
void callbackSimulationState (const std_msgs::Int32 &msg)
 
void callbackSimulationStepDone (const std_msgs::Bool &msg)
 
void callbackSimulationTime (const std_msgs::Float32 &msg)
 
void positionControlLoop ()
 
void readingLoop ()
 
void torqueControlLoop ()
 
void velocityControlLoop ()
 
- Protected Member Functions inherited from vpRobotFrankaSim
vpColVector getVelDes ()
 
vpColVector solveIK (const vpHomogeneousMatrix &edMw)
 

Protected Attributes

std::thread m_acquisitionThread
 
bool m_connected
 
std::thread m_controlThread
 
std::atomic_bool m_ftControlThreadIsRunning
 
std::atomic_bool m_ftControlThreadStopAsked
 
bool m_overwrite_flMe
 
bool m_overwrite_toolInertia
 
std::atomic_bool m_posControlLock
 
std::atomic_bool m_posControlNewCmd
 
std::atomic_bool m_posControlThreadIsRunning
 
std::atomic_bool m_posControlThreadStopAsked
 
ros::Publisher m_pub_enableSyncMode
 
ros::Publisher m_pub_jointStateCmd
 
ros::Publisher m_pub_pauseSimulation
 
ros::Publisher m_pub_robotStateCmd
 
ros::Publisher m_pub_startSimulation
 
ros::Publisher m_pub_stopSimulation
 
ros::Publisher m_pub_triggerNextStep
 
int m_simulationState
 
bool m_simulationStepDone
 
float m_simulationTime
 
ros::Subscriber m_sub_coppeliasim_eMc
 
ros::Subscriber m_sub_coppeliasim_flMe
 
ros::Subscriber m_sub_coppeliasim_g0
 
ros::Subscriber m_sub_coppeliasim_jointState
 
ros::Subscriber m_sub_coppeliasim_simulationState
 
ros::Subscriber m_sub_coppeliasim_simulationStepDone
 
ros::Subscriber m_sub_coppeliasim_simulationTime
 
ros::Subscriber m_sub_coppeliasim_toolInertia
 
bool m_syncModeEnabled
 
std::string m_topic_eMc
 
std::string m_topic_flMcom
 
std::string m_topic_flMe
 
std::string m_topic_g0
 
std::string m_topic_jointState
 
std::string m_topic_jointStateCmd
 
std::string m_topic_robotStateCmd
 
std::string m_topic_toolInertia
 
std::atomic_bool m_velControlThreadIsRunning
 
std::atomic_bool m_velControlThreadStopAsked
 
- Protected Attributes inherited from vpRobotFrankaSim
bool m_camMounted
 
KDL::Chain m_chain_kdl
 
KDL::ChainIkSolverVel_pinvm_diffIkSolver_kdl
 
vpColVector m_dq
 
vpColVector m_dq_des
 
vpColVector m_dq_des_filt
 
KDL::JntArray m_dq_des_kdl
 
vpHomogeneousMatrix m_eMc
 
vpVelocityTwistMatrix m_eVc
 
KDL::ChainFkSolverPos_recursivem_fksolver_kdl
 
vpHomogeneousMatrix m_flMcom
 
vpHomogeneousMatrix m_flMe
 
vpColVector m_g0
 
KDL::ChainIkSolverPos_NR_JLm_iksolver_JL_kdl
 
vpMatrix m_Il
 
KDL::ChainJntToJacSolverm_jacobianSolver_kdl
 
double m_mL
 
std::mutex m_mutex
 
vpColVector m_q
 
vpColVector m_q_des
 
KDL::JntArray m_q_kdl
 
KDL::JntArray m_q_max_kdl
 
KDL::JntArray m_q_min_kdl
 
vpRobot::vpRobotStateType m_stateRobot
 
vpColVector m_tau_J
 
vpColVector m_tau_J_des
 
vpColVector m_tau_J_des_filt
 
bool m_toolMounted
 
vpColVector m_v_cart_des
 
bool m_verbose
 

Detailed Description

This class does the connexion using ROS 1 between Franka robot simulator implemented in vpRobotFrankaSim and Coppeliasim simulator.

It subscribes to default topics emited and updated on Coppeliasim side:

Given the type of command to apply to the simulated robot, it published default topics that are taken into account by Coppeliasim to update the robot state:

Note that changing topic names should be achieved before calling connect().

robot.setTopicJointState("/coppeliasim/franka/joint_state");
robot.setTopic_eMc("/coppeliasim/franka/eMc");
robot.setTopicJointStateCmd("/fakeFCI/joint_state");
robot.setTopicRobotStateCmd("/fakeFCI/robot_state");
robot.connect();

If you decide to change the topic names, be aware that you should also change their names in Coppeliasim scene lua script.

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 87 of file vpROSRobotFrankaCoppeliasim.h.

Constructor & Destructor Documentation

◆ vpROSRobotFrankaCoppeliasim()

vpROSRobotFrankaCoppeliasim::vpROSRobotFrankaCoppeliasim ( )

Default constructor.

Definition at line 52 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ ~vpROSRobotFrankaCoppeliasim()

vpROSRobotFrankaCoppeliasim::~vpROSRobotFrankaCoppeliasim ( )
virtual

Destructor.

Definition at line 98 of file vpROSRobotFrankaCoppeliasim.cpp.

Member Function Documentation

◆ callback_eMc()

void vpROSRobotFrankaCoppeliasim::callback_eMc ( const geometry_msgs::Pose pose_msg)
protected

Callback that updates camera extrinsics with the transformation considered on Coppeliasim side.

Parameters
[in]pose_msg: Message associated to the topic set by setTopic_eMc() that contains the homogeneous transformation between end-effector and camera frame.

Definition at line 434 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ callback_flMe()

void vpROSRobotFrankaCoppeliasim::callback_flMe ( const geometry_msgs::Pose pose_msg)
protected

Callback that updates end-effector extrinsics w.r.t. the robot flange with the transformation considered on Coppeliasim side.

Parameters
[in]pose_msg: Message associated to the topic set by setTopic_flMe() that contains the homogeneous transformation between end-effector and flange frame. This callback works jointly with the callback_toolInertia to automatically retrieve the kinematic and dynamic parameters of the tool.

Definition at line 451 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ callback_g0()

void vpROSRobotFrankaCoppeliasim::callback_g0 ( const geometry_msgs::Vector3 &  g0_msg)
protected

Callback that updates the gravitational acceleration vector as perceived at the robot's base considered on Coppeliasim side.

Parameters
[in]g0_msg: Message associated to the topic set by setTopic_g0() the absolute acceleration vector in base frame.

Definition at line 418 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ callback_toolInertia()

void vpROSRobotFrankaCoppeliasim::callback_toolInertia ( const geometry_msgs::Inertia &  inertia_msg)
protected

Callback that updates the tool inertial parameters.

Parameters
[in]inertia_msg: Message associated to the topic set by setTopic_toolInertia() that contains the inertial parameters of the mounted tool as considered on Coppeliasim side. The inertia tensor must be specified in flange frame as well as the center of mass. This callback works jointly with the callback_flMe to automatically retrieve the kinematic and dynamic parameters of the tool.

Definition at line 483 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ callbackJointState()

void vpROSRobotFrankaCoppeliasim::callbackJointState ( const sensor_msgs::JointState &  joint_state)
protected

Callback that updates robot joint state (joint position, joint velocity, joint torque) with the values updated on Coppeliasim side.

Parameters
[in]joint_state: Joint state message associated to the topic set by setTopicJointState().

Definition at line 400 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ callbackSimulationState()

void vpROSRobotFrankaCoppeliasim::callbackSimulationState ( const std_msgs::Int32 &  msg)
protected

Callback that allows to update the Coppeliasim simulation state.

Parameters
[in]msg: Simulation state message.

Definition at line 353 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ callbackSimulationStepDone()

void vpROSRobotFrankaCoppeliasim::callbackSimulationStepDone ( const std_msgs::Bool &  msg)
protected

Callback that allows to know when a simulation step is done on Coppeliasim side.

Parameters
[in]msg: Simulation step done message send by Coppeliasim

Definition at line 299 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ callbackSimulationTime()

void vpROSRobotFrankaCoppeliasim::callbackSimulationTime ( const std_msgs::Float32 &  msg)
protected

Callback that allows to update the Coppeliasim simulation time.

Parameters
[in]msg: Simulation time message.

Definition at line 332 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ connect()

void vpROSRobotFrankaCoppeliasim::connect ( const std::string &  robot_ID = "")

Connect to Coppeliasim simulator by subscribing to the requested topics.

It is possible to change the default topic names, before calling this function.

See also
setTopicJointState(), setTopic_eMc(), setTopicJointStateCmd(), setTopicRobotStateCmdCmd()
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 132 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ coppeliasimPauseSimulation()

void vpROSRobotFrankaCoppeliasim::coppeliasimPauseSimulation ( double  sleep_ms = 1000.)

Pause Coppeliasim simulation by publishing a std_msgs::Bool message on /pauseSimulation topic.

Parameters
[in]sleep_ms: Sleeping time in [ms] added after publishing the message to ensure that it will be taken into account by Coppeliasim.
See also
coppeliasimStartSimulation(), coppeliasimStopSimulation(), getCoppeliasimSimulationState()

Definition at line 229 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ coppeliasimStartSimulation()

void vpROSRobotFrankaCoppeliasim::coppeliasimStartSimulation ( double  sleep_ms = 1000.)

Start Coppeliasim simulation by publishing a a std_msgs::Bool message on /startSimulation topic.

Parameters
[in]sleep_ms: Sleeping time in [ms] added after publishing the message to ensure that it will be taken into account by Coppeliasim.
See also
coppeliasimPauseSimulation(), coppeliasimStopSimulation(), getCoppeliasimSimulationState()
Examples:
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 248 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ coppeliasimStopSimulation()

void vpROSRobotFrankaCoppeliasim::coppeliasimStopSimulation ( double  sleep_ms = 1000.)

Stop Coppeliasim simulation by publishing a std_msgs::Bool message on /stopSimulation topic.

Parameters
[in]sleep_ms: Sleeping time in [ms] added after publishing the message to ensure that it will be taken into account by Coppeliasim.
See also
coppeliasimStartSimulation(), coppeliasimPauseSimulation(), getCoppeliasimSimulationState()
Examples:
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 268 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ coppeliasimTriggerNextStep()

void vpROSRobotFrankaCoppeliasim::coppeliasimTriggerNextStep ( )

Trigger Coppeliasim next simulation step by publishing a std_msgs::Bool message on /triggerNextStep topic, while in the synchronous simulation mode.

Note
It is not useful to call this method when Coppeliasim synchronous mode is not set using setCoppeliasimSyncMode().
See also
setCoppeliasimSyncMode()

Definition at line 287 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ getCoppeliasimSimulationState()

int vpROSRobotFrankaCoppeliasim::getCoppeliasimSimulationState ( )

Return Coppeliasim simulation state. 0 indicates that the simulation is stopped, 1 that it is running and 2 that it is paused.

See also
coppeliasimStartSimulation(), coppeliasimPauseSimulation(), coppeliasimStopSimulation()

Definition at line 367 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ getCoppeliasimSimulationStepDone()

bool vpROSRobotFrankaCoppeliasim::getCoppeliasimSimulationStepDone ( )

Return true when a simulation step is done on Coppeliasim side, false otherwise.

Definition at line 309 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ getCoppeliasimSimulationTime()

double vpROSRobotFrankaCoppeliasim::getCoppeliasimSimulationTime ( )

◆ getCoppeliasimSyncMode()

bool vpROSRobotFrankaCoppeliasim::getCoppeliasimSyncMode ( )
inline

Get Coppeliasim synchronous mode.

Returns
true when synchronous mode is enable, false otherwise.
See also
setCoppeliasimSyncMode()

Definition at line 110 of file vpROSRobotFrankaCoppeliasim.h.

◆ isConnected()

bool vpROSRobotFrankaCoppeliasim::isConnected ( ) const
inline

Return true when ROS connexion with Coppeliasim is established.

Definition at line 115 of file vpROSRobotFrankaCoppeliasim.h.

◆ positionControlLoop()

void vpROSRobotFrankaCoppeliasim::positionControlLoop ( )
protected

Infinite loop launched when the control is done in position.

Definition at line 527 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ readingLoop()

void vpROSRobotFrankaCoppeliasim::readingLoop ( )
protected

Subscribes to topics updated on Coppeliasim side and runs an infinite loop that updates the robot state.

See also
setTopicJointState(), setTopic_eMc()

Definition at line 380 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ setCoppeliasimSimulationStepDone()

void vpROSRobotFrankaCoppeliasim::setCoppeliasimSimulationStepDone ( bool  simulationStepDone)

Set Coppeliasim simulation step done flag.

Parameters
[in]simulationStepDone: True to indicate that the simmulation step is done, false otherwise.

Definition at line 321 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ setCoppeliasimSyncMode()

void vpROSRobotFrankaCoppeliasim::setCoppeliasimSyncMode ( bool  enable,
double  sleep_ms = 1000. 
)

Enable/disable Coppeliasim synchronous simulation mode by publishing a std_msgs::Bool message on /enableSyncMode topic.

Parameters
[in]enable: Set true to enable synchronous simulation mode, false otherwise. When synchronous simulation mode is enable, you need to call coppeliasimTriggerNextStep() to run a new simulation step.
[in]sleep_ms: Sleeping time in [ms] added after publishing the message to ensure that it will be taken into account by Coppeliasim.
See also
coppeliasimTriggerNextStep(), getCoppeliasimSybcMode()
Examples:
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 209 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ setPosition()

void vpROSRobotFrankaCoppeliasim::setPosition ( const vpRobot::vpControlFrameType  frame,
const vpColVector &  position 
)
virtual

Set robot position. This function is blocking and returns only when the desired position is reached.

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 from vpRobotFrankaSim.

Examples:
tutorial-franka-coppeliasim-cartesian-impedance-control.cpp, tutorial-franka-coppeliasim-ibvs-apriltag.cpp, tutorial-franka-coppeliasim-joint-impedance-control.cpp, and tutorial-franka-coppeliasim-pbvs-apriltag.cpp.

Definition at line 947 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ setRobotState()

vpRobot::vpRobotStateType vpROSRobotFrankaCoppeliasim::setRobotState ( vpRobot::vpRobotStateType  newState)

◆ setTopic_eMc()

void vpROSRobotFrankaCoppeliasim::setTopic_eMc ( const std::string &  topic_eMc)
inline

Set topic name that contains eMc corresponding to the end-effector to camera transformation.

Parameters
topic_eMc: Topic name.
Examples:
test-vel.cpp.

Definition at line 171 of file vpROSRobotFrankaCoppeliasim.h.

◆ setTopic_flMcom()

void vpROSRobotFrankaCoppeliasim::setTopic_flMcom ( const std::string &  topic_flMcom)
inline

Set topic name that contains flMcom corresponding to the flange to Center-of-Mass transformation.

Parameters
topic_flMcom: Topic name.

Definition at line 187 of file vpROSRobotFrankaCoppeliasim.h.

◆ setTopic_flMe()

void vpROSRobotFrankaCoppeliasim::setTopic_flMe ( const std::string &  topic_flMe)
inline

Set topic name that contains flMe corresponding to the flange to end-effector transformation.

Parameters
topic_flMe: Topic name.

Definition at line 179 of file vpROSRobotFrankaCoppeliasim.h.

◆ setTopic_g0()

void vpROSRobotFrankaCoppeliasim::setTopic_g0 ( const std::string &  topic_g0)
inline

Set topic name that contains g0 corresponding to the absolute acceleration vector at the robot's base.

Parameters
topic_g0: Topic name.

Definition at line 163 of file vpROSRobotFrankaCoppeliasim.h.

◆ setTopic_toolInertia()

void vpROSRobotFrankaCoppeliasim::setTopic_toolInertia ( const std::string &  topic_toolInertia)
inline

Set topic name that contains the Inertia parameters of the tool.

Parameters
topic_toolInertia: Topic name.

Definition at line 194 of file vpROSRobotFrankaCoppeliasim.h.

◆ setTopicJointState()

void vpROSRobotFrankaCoppeliasim::setTopicJointState ( const std::string &  topic_jointState)
inline

Set topic name that contains the robot state including position, velocity and effort for all the joints.

Parameters
topic_jointState: Topic name.
Examples:
test-vel.cpp.

Definition at line 130 of file vpROSRobotFrankaCoppeliasim.h.

◆ setTopicJointStateCmd()

void vpROSRobotFrankaCoppeliasim::setTopicJointStateCmd ( const std::string &  topic_jointStateCmd)
inline

Name of the topic used to public joint state command that has to be applied to the robot.

Parameters
topic_jointStateCmd: Topic name.

Definition at line 141 of file vpROSRobotFrankaCoppeliasim.h.

◆ setTopicRobotStateCmd()

void vpROSRobotFrankaCoppeliasim::setTopicRobotStateCmd ( const std::string &  topic_robotState)
inline

Set topic name that contains the robot controller state (stopped, position, velocity, force/torque).

Parameters
topic_robotState: Topic name.

Definition at line 152 of file vpROSRobotFrankaCoppeliasim.h.

◆ torqueControlLoop()

void vpROSRobotFrankaCoppeliasim::torqueControlLoop ( )
protected

Infinite loop launched when the control is done in joint torque.

Definition at line 680 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ velocityControlLoop()

void vpROSRobotFrankaCoppeliasim::velocityControlLoop ( )
protected

Infinite loop launched when the control is done in joint velocity.

Definition at line 619 of file vpROSRobotFrankaCoppeliasim.cpp.

◆ wait()

void vpROSRobotFrankaCoppeliasim::wait ( double  timestamp_second,
double  duration_second 
)

Wait a given time in [s].

Parameters
timestamp_second: Simulation time in [s] used to start the chrono.
duration_second: Wait the given duration time in [s] since chrono is started
Examples:
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 976 of file vpROSRobotFrankaCoppeliasim.cpp.

Member Data Documentation

◆ m_acquisitionThread

std::thread vpROSRobotFrankaCoppeliasim::m_acquisitionThread
protected

Definition at line 217 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_connected

bool vpROSRobotFrankaCoppeliasim::m_connected
protected

Definition at line 230 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_controlThread

std::thread vpROSRobotFrankaCoppeliasim::m_controlThread
protected

Definition at line 216 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_ftControlThreadIsRunning

std::atomic_bool vpROSRobotFrankaCoppeliasim::m_ftControlThreadIsRunning
protected

Definition at line 243 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_ftControlThreadStopAsked

std::atomic_bool vpROSRobotFrankaCoppeliasim::m_ftControlThreadStopAsked
protected

Definition at line 244 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_overwrite_flMe

bool vpROSRobotFrankaCoppeliasim::m_overwrite_flMe
protected

Definition at line 273 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_overwrite_toolInertia

bool vpROSRobotFrankaCoppeliasim::m_overwrite_toolInertia
protected

Definition at line 271 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_posControlLock

std::atomic_bool vpROSRobotFrankaCoppeliasim::m_posControlLock
protected

Definition at line 235 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_posControlNewCmd

std::atomic_bool vpROSRobotFrankaCoppeliasim::m_posControlNewCmd
protected

Definition at line 236 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_posControlThreadIsRunning

std::atomic_bool vpROSRobotFrankaCoppeliasim::m_posControlThreadIsRunning
protected

Definition at line 233 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_posControlThreadStopAsked

std::atomic_bool vpROSRobotFrankaCoppeliasim::m_posControlThreadStopAsked
protected

Definition at line 234 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_pub_enableSyncMode

ros::Publisher vpROSRobotFrankaCoppeliasim::m_pub_enableSyncMode
protected

Definition at line 253 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_pub_jointStateCmd

ros::Publisher vpROSRobotFrankaCoppeliasim::m_pub_jointStateCmd
protected

Definition at line 247 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_pub_pauseSimulation

ros::Publisher vpROSRobotFrankaCoppeliasim::m_pub_pauseSimulation
protected

Definition at line 250 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_pub_robotStateCmd

ros::Publisher vpROSRobotFrankaCoppeliasim::m_pub_robotStateCmd
protected

Definition at line 248 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_pub_startSimulation

ros::Publisher vpROSRobotFrankaCoppeliasim::m_pub_startSimulation
protected

Definition at line 249 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_pub_stopSimulation

ros::Publisher vpROSRobotFrankaCoppeliasim::m_pub_stopSimulation
protected

Definition at line 251 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_pub_triggerNextStep

ros::Publisher vpROSRobotFrankaCoppeliasim::m_pub_triggerNextStep
protected

Definition at line 252 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_simulationState

int vpROSRobotFrankaCoppeliasim::m_simulationState
protected

Definition at line 269 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_simulationStepDone

bool vpROSRobotFrankaCoppeliasim::m_simulationStepDone
protected

Definition at line 266 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_simulationTime

float vpROSRobotFrankaCoppeliasim::m_simulationTime
protected

Definition at line 267 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_sub_coppeliasim_eMc

ros::Subscriber vpROSRobotFrankaCoppeliasim::m_sub_coppeliasim_eMc
protected

Definition at line 258 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_sub_coppeliasim_flMe

ros::Subscriber vpROSRobotFrankaCoppeliasim::m_sub_coppeliasim_flMe
protected

Definition at line 259 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_sub_coppeliasim_g0

ros::Subscriber vpROSRobotFrankaCoppeliasim::m_sub_coppeliasim_g0
protected

Definition at line 257 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_sub_coppeliasim_jointState

ros::Subscriber vpROSRobotFrankaCoppeliasim::m_sub_coppeliasim_jointState
protected

Definition at line 256 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_sub_coppeliasim_simulationState

ros::Subscriber vpROSRobotFrankaCoppeliasim::m_sub_coppeliasim_simulationState
protected

Definition at line 263 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_sub_coppeliasim_simulationStepDone

ros::Subscriber vpROSRobotFrankaCoppeliasim::m_sub_coppeliasim_simulationStepDone
protected

Definition at line 261 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_sub_coppeliasim_simulationTime

ros::Subscriber vpROSRobotFrankaCoppeliasim::m_sub_coppeliasim_simulationTime
protected

Definition at line 262 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_sub_coppeliasim_toolInertia

ros::Subscriber vpROSRobotFrankaCoppeliasim::m_sub_coppeliasim_toolInertia
protected

Definition at line 260 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_syncModeEnabled

bool vpROSRobotFrankaCoppeliasim::m_syncModeEnabled
protected

Definition at line 268 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_topic_eMc

std::string vpROSRobotFrankaCoppeliasim::m_topic_eMc
protected

Definition at line 222 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_topic_flMcom

std::string vpROSRobotFrankaCoppeliasim::m_topic_flMcom
protected

Definition at line 224 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_topic_flMe

std::string vpROSRobotFrankaCoppeliasim::m_topic_flMe
protected

Definition at line 223 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_topic_g0

std::string vpROSRobotFrankaCoppeliasim::m_topic_g0
protected

Definition at line 221 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_topic_jointState

std::string vpROSRobotFrankaCoppeliasim::m_topic_jointState
protected

Definition at line 220 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_topic_jointStateCmd

std::string vpROSRobotFrankaCoppeliasim::m_topic_jointStateCmd
protected

Definition at line 227 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_topic_robotStateCmd

std::string vpROSRobotFrankaCoppeliasim::m_topic_robotStateCmd
protected

Definition at line 228 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_topic_toolInertia

std::string vpROSRobotFrankaCoppeliasim::m_topic_toolInertia
protected

Definition at line 225 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_velControlThreadIsRunning

std::atomic_bool vpROSRobotFrankaCoppeliasim::m_velControlThreadIsRunning
protected

Definition at line 239 of file vpROSRobotFrankaCoppeliasim.h.

◆ m_velControlThreadStopAsked

std::atomic_bool vpROSRobotFrankaCoppeliasim::m_velControlThreadStopAsked
protected

Definition at line 240 of file vpROSRobotFrankaCoppeliasim.h.


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 Tue Mar 1 2022 00:03:22