39 #include <visp3/core/vpConfig.h>
41 #if defined(VISP_HAVE_UR_RTDE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
43 #include <visp3/core/vpIoTools.h>
44 #include <visp3/robot/vpRobotUniversalRobots.h>
72 : m_rtde_receive(nullptr), m_rtde_control(nullptr), m_eMc()
87 m_rtde_receive = std::make_shared<ur_rtde::RTDEReceiveInterface>(ur_address);
90 m_rtde_control = std::make_shared<ur_rtde::RTDEControlInterface>(ur_address);
93 m_db_client = std::make_shared<ur_rtde::DashboardClient>(ur_address);
183 "Cannot get UR robot forward kinematics: joint position vector is not 6-dim (%d)", q.
size()));
192 std::vector<double> tcp_pose =
m_rtde_control->getForwardKinematics(q_std);
195 for (
size_t i = 0; i < 6; i++) {
196 f_P_e[i] = tcp_pose[i];
329 std::vector<double> tcp_pose =
m_rtde_receive->getActualTCPPose();
331 for (
size_t i = 0; i < tcp_pose.size(); i++) {
332 position[i] = tcp_pose[i];
338 std::vector<double> tcp_pose =
m_rtde_receive->getActualTCPPose();
340 for (
size_t i = 0; i < tcp_pose.size(); i++) {
341 fPe[i] = tcp_pose[i];
348 for (
size_t i = 0; i < 6; i++) {
349 position[i] = fPc[i];
382 for (
size_t i = 0; i < 6; i++) {
383 pose[i] = position[i];
472 if (position.
size() != 6) {
474 "Cannot set UR robot position: position vector is not a 6-dim vector (%d)", position.
size()));
478 std::cout <<
"Robot is not in position-based control. "
479 "Modification of the robot state"
486 std::vector<double> new_q = position.
toStdVector();
490 std::vector<double> new_pose = position.
toStdVector();
506 "Cannot move the robot to a cartesian position. Only joint positioning is implemented"));
601 "Cannot send a velocity to the robot: robot is not in velocity control state "
602 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
617 for (
unsigned int i = 0; i < 3; i++)
619 for (
unsigned int i = 3; i < 6; i++)
635 double dt = 1.0 / 1000;
636 double acceleration = 0.5;
639 double dt = 1.0 / 1000;
640 double acceleration = 0.25;
643 double dt = 1.0 / 1000;
644 double acceleration = 0.25;
646 m_rtde_control->speedL((fVe * vel_sat).toStdVector(), acceleration, dt);
648 double dt = 1.0 / 1000;
649 double acceleration = 0.25;
654 "Cannot move the robot in velocity in the specified frame: not implemented"));
731 std::ifstream fd(filename.c_str(), std::ios::in);
738 std::string key(
"R:");
739 std::string id(
"#UR - Joint position file");
740 bool pos_found =
false;
742 size_t njoints =
static_cast<size_t>(
nDof);
746 while (std::getline(fd, line)) {
749 if (!(line.compare(0,
id.size(),
id) == 0)) {
750 std::cout <<
"Error: this position file " << filename <<
" is not for Universal Robots" << std::endl;
754 if ((line.compare(0, 1,
"#") == 0)) {
757 if ((line.compare(0, key.size(), key) == 0)) {
760 if (chain.size() < njoints + 1)
762 if (chain.size() < njoints + 1)
765 std::istringstream ss(line);
768 for (
unsigned int i = 0; i < njoints; i++)
776 for (
unsigned int i = 0; i < njoints; i++) {
783 std::cout <<
"Error: unable to find a position for UR robot in " << filename << std::endl;
811 fd = fopen(filename.c_str(),
"w");
815 fprintf(fd,
"#UR - Joint position file\n"
817 "# R: q1 q2 q3 q4 q5 q6\n"
818 "# with joint positions q1 to q6 expressed in degrees\n"
860 std::cout <<
"Change the control mode from stop to velocity control.\n";
871 #elif !defined(VISP_BUILD_SHARED_LIBS)
874 void dummy_vpRobotUniversalRobots(){};
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
vpColVector extract(unsigned int r, unsigned int colsize) const
std::vector< double > toStdVector() const
void resize(unsigned int i, bool flagNullify=true)
error that can be emited by ViSP classes.
@ functionNotImplementedError
Function not implemented.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
static double deg(double rad)
Implementation of a pose vector and operations on poses.
std::vector< double > toStdVector() const
Error that can be emited by the vpRobot class and its derivates.
double m_max_joint_acceleration
vpHomogeneousMatrix get_eMc() const
vpHomogeneousMatrix m_eMc
double m_positioningVelocity
void setPositioningVelocity(double velocity)
std::string getRobotModel() const
vpHomogeneousMatrix get_fMc()
vpHomogeneousMatrix get_fMe()
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
double m_max_linear_speed
void move(const std::string &filename, double velocity_percentage=10.)
virtual ~vpRobotUniversalRobots()
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void connect(const std::string &ur_address)
double m_max_linear_acceleration
bool savePosFile(const std::string &filename, const vpColVector &q)
void set_eMc(const vpHomogeneousMatrix &eMc)
std::shared_ptr< ur_rtde::DashboardClient > m_db_client
vpRobot::vpControlFrameType m_vel_control_frame
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void getForceTorque(const vpRobot::vpControlFrameType frame, vpColVector &force)
std::shared_ptr< ur_rtde::RTDEReceiveInterface > m_rtde_receive
std::shared_ptr< ur_rtde::RTDEControlInterface > m_rtde_control
std::string getPolyScopeVersion()
bool readPosFile(const std::string &filename, vpColVector &q)
int nDof
number of degrees of freedom
virtual vpRobotStateType getRobotState(void) const
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
double getMaxRotationVelocity(void) const
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
double getMaxTranslationVelocity(void) const
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.