mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 01:50:46 +02:00
Clean up of old driver files
This commit is contained in:
Simon Rasmussen
@@ -1,8 +0,0 @@
|
||||
#pragma once
|
||||
#include "ur_modern_driver/bin_parser.h"
|
||||
|
||||
class Packet
|
||||
{
|
||||
public:
|
||||
virtual bool parseWith(BinParser& bp) = 0;
|
||||
};
|
||||
@@ -1,9 +0,0 @@
|
||||
#pragma once
|
||||
#include "ur_modern_driver/bin_parser.h"
|
||||
#include "ur_modern_driver/packet.h"
|
||||
|
||||
class Parser
|
||||
{
|
||||
public:
|
||||
virtual std::unique_ptr<Packet> parse(BinParser& bp) = 0;
|
||||
};
|
||||
@@ -1,233 +0,0 @@
|
||||
/*
|
||||
* robot_state.h
|
||||
*
|
||||
* Copyright 2015 Thomas Timm Andersen
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef ROBOT_STATE_H_
|
||||
#define ROBOT_STATE_H_
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <netinet/in.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
|
||||
namespace message_types
|
||||
{
|
||||
enum message_type
|
||||
{
|
||||
ROBOT_STATE = 16,
|
||||
ROBOT_MESSAGE = 20,
|
||||
PROGRAM_STATE_MESSAGE = 25
|
||||
};
|
||||
}
|
||||
typedef message_types::message_type messageType;
|
||||
|
||||
namespace package_types
|
||||
{
|
||||
enum package_type
|
||||
{
|
||||
ROBOT_MODE_DATA = 0,
|
||||
JOINT_DATA = 1,
|
||||
TOOL_DATA = 2,
|
||||
MASTERBOARD_DATA = 3,
|
||||
CARTESIAN_INFO = 4,
|
||||
KINEMATICS_INFO = 5,
|
||||
CONFIGURATION_DATA = 6,
|
||||
FORCE_MODE_DATA = 7,
|
||||
ADDITIONAL_INFO = 8,
|
||||
CALIBRATION_DATA = 9
|
||||
};
|
||||
}
|
||||
typedef package_types::package_type packageType;
|
||||
|
||||
namespace robot_message_types
|
||||
{
|
||||
enum robot_message_type
|
||||
{
|
||||
ROBOT_MESSAGE_TEXT = 0,
|
||||
ROBOT_MESSAGE_PROGRAM_LABEL = 1,
|
||||
PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
|
||||
ROBOT_MESSAGE_VERSION = 3,
|
||||
ROBOT_MESSAGE_SAFETY_MODE = 5,
|
||||
ROBOT_MESSAGE_ERROR_CODE = 6,
|
||||
ROBOT_MESSAGE_KEY = 7,
|
||||
ROBOT_MESSAGE_REQUEST_VALUE = 9,
|
||||
ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
|
||||
};
|
||||
}
|
||||
typedef robot_message_types::robot_message_type robotMessageType;
|
||||
|
||||
namespace robot_state_type_v18
|
||||
{
|
||||
enum robot_state_type
|
||||
{
|
||||
ROBOT_RUNNING_MODE = 0,
|
||||
ROBOT_FREEDRIVE_MODE = 1,
|
||||
ROBOT_READY_MODE = 2,
|
||||
ROBOT_INITIALIZING_MODE = 3,
|
||||
ROBOT_SECURITY_STOPPED_MODE = 4,
|
||||
ROBOT_EMERGENCY_STOPPED_MODE = 5,
|
||||
ROBOT_FATAL_ERROR_MODE = 6,
|
||||
ROBOT_NO_POWER_MODE = 7,
|
||||
ROBOT_NOT_CONNECTED_MODE = 8,
|
||||
ROBOT_SHUTDOWN_MODE = 9,
|
||||
ROBOT_SAFEGUARD_STOP_MODE = 10
|
||||
};
|
||||
}
|
||||
typedef robot_state_type_v18::robot_state_type robotStateTypeV18;
|
||||
namespace robot_state_type_v30
|
||||
{
|
||||
enum robot_state_type
|
||||
{
|
||||
ROBOT_MODE_DISCONNECTED = 0,
|
||||
ROBOT_MODE_CONFIRM_SAFETY = 1,
|
||||
ROBOT_MODE_BOOTING = 2,
|
||||
ROBOT_MODE_POWER_OFF = 3,
|
||||
ROBOT_MODE_POWER_ON = 4,
|
||||
ROBOT_MODE_IDLE = 5,
|
||||
ROBOT_MODE_BACKDRIVE = 6,
|
||||
ROBOT_MODE_RUNNING = 7,
|
||||
ROBOT_MODE_UPDATING_FIRMWARE = 8
|
||||
};
|
||||
}
|
||||
|
||||
typedef robot_state_type_v30::robot_state_type robotStateTypeV30;
|
||||
|
||||
struct version_message
|
||||
{
|
||||
uint64_t timestamp;
|
||||
int8_t source;
|
||||
int8_t robot_message_type;
|
||||
int8_t project_name_size;
|
||||
char project_name[15];
|
||||
uint8_t major_version;
|
||||
uint8_t minor_version;
|
||||
int svn_revision;
|
||||
char build_date[25];
|
||||
};
|
||||
|
||||
struct masterboard_data
|
||||
{
|
||||
int digitalInputBits;
|
||||
int digitalOutputBits;
|
||||
char analogInputRange0;
|
||||
char analogInputRange1;
|
||||
double analogInput0;
|
||||
double analogInput1;
|
||||
char analogOutputDomain0;
|
||||
char analogOutputDomain1;
|
||||
double analogOutput0;
|
||||
double analogOutput1;
|
||||
float masterBoardTemperature;
|
||||
float robotVoltage48V;
|
||||
float robotCurrent;
|
||||
float masterIOCurrent;
|
||||
unsigned char safetyMode;
|
||||
unsigned char masterOnOffState;
|
||||
char euromap67InterfaceInstalled;
|
||||
int euromapInputBits;
|
||||
int euromapOutputBits;
|
||||
float euromapVoltage;
|
||||
float euromapCurrent;
|
||||
};
|
||||
|
||||
struct robot_mode_data
|
||||
{
|
||||
uint64_t timestamp;
|
||||
bool isRobotConnected;
|
||||
bool isRealRobotEnabled;
|
||||
bool isPowerOnRobot;
|
||||
bool isEmergencyStopped;
|
||||
bool isProtectiveStopped;
|
||||
bool isProgramRunning;
|
||||
bool isProgramPaused;
|
||||
unsigned char robotMode;
|
||||
unsigned char controlMode;
|
||||
double targetSpeedFraction;
|
||||
double speedScaling;
|
||||
};
|
||||
|
||||
class RobotState
|
||||
{
|
||||
private:
|
||||
version_message version_msg_;
|
||||
masterboard_data mb_data_;
|
||||
robot_mode_data robot_mode_;
|
||||
|
||||
std::recursive_mutex val_lock_; // Locks the variables while unpack parses data;
|
||||
|
||||
std::condition_variable* pMsg_cond_; // Signals that new vars are available
|
||||
bool new_data_available_; // to avoid spurious wakes
|
||||
unsigned char robot_mode_running_;
|
||||
|
||||
double ntohd(uint64_t nf);
|
||||
|
||||
public:
|
||||
RobotState(std::condition_variable& msg_cond);
|
||||
~RobotState();
|
||||
double getVersion();
|
||||
double getTime();
|
||||
std::vector<double> getQTarget();
|
||||
int getDigitalInputBits();
|
||||
int getDigitalOutputBits();
|
||||
char getAnalogInputRange0();
|
||||
char getAnalogInputRange1();
|
||||
double getAnalogInput0();
|
||||
double getAnalogInput1();
|
||||
char getAnalogOutputDomain0();
|
||||
char getAnalogOutputDomain1();
|
||||
double getAnalogOutput0();
|
||||
double getAnalogOutput1();
|
||||
std::vector<double> getVActual();
|
||||
float getMasterBoardTemperature();
|
||||
float getRobotVoltage48V();
|
||||
float getRobotCurrent();
|
||||
float getMasterIOCurrent();
|
||||
unsigned char getSafetyMode();
|
||||
unsigned char getInReducedMode();
|
||||
char getEuromap67InterfaceInstalled();
|
||||
int getEuromapInputBits();
|
||||
int getEuromapOutputBits();
|
||||
float getEuromapVoltage();
|
||||
float getEuromapCurrent();
|
||||
|
||||
bool isRobotConnected();
|
||||
bool isRealRobotEnabled();
|
||||
bool isPowerOnRobot();
|
||||
bool isEmergencyStopped();
|
||||
bool isProtectiveStopped();
|
||||
bool isProgramRunning();
|
||||
bool isProgramPaused();
|
||||
unsigned char getRobotMode();
|
||||
bool isReady();
|
||||
|
||||
void setDisconnected();
|
||||
|
||||
bool getNewDataAvailable();
|
||||
void finishedReading();
|
||||
|
||||
void unpack(uint8_t* buf, unsigned int buf_length);
|
||||
void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len);
|
||||
void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len);
|
||||
void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len);
|
||||
void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset);
|
||||
void unpackRobotMode(uint8_t* buf, unsigned int offset);
|
||||
};
|
||||
|
||||
#endif /* ROBOT_STATE_H_ */
|
||||
@@ -1,119 +0,0 @@
|
||||
/*
|
||||
* robotStateRT.h
|
||||
*
|
||||
* Copyright 2015 Thomas Timm Andersen
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef ROBOT_STATE_RT_H_
|
||||
#define ROBOT_STATE_RT_H_
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <netinet/in.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
|
||||
class RobotStateRT
|
||||
{
|
||||
private:
|
||||
double version_; // protocol version
|
||||
|
||||
double time_; // Time elapsed since the controller was started
|
||||
std::vector<double> q_target_; // Target joint positions
|
||||
std::vector<double> qd_target_; // Target joint velocities
|
||||
std::vector<double> qdd_target_; // Target joint accelerations
|
||||
std::vector<double> i_target_; // Target joint currents
|
||||
std::vector<double> m_target_; // Target joint moments (torques)
|
||||
std::vector<double> q_actual_; // Actual joint positions
|
||||
std::vector<double> qd_actual_; // Actual joint velocities
|
||||
std::vector<double> i_actual_; // Actual joint currents
|
||||
std::vector<double> i_control_; // Joint control currents
|
||||
std::vector<double> tool_vector_actual_; // Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry
|
||||
// and rz is a rotation vector representation of the tool orientation
|
||||
std::vector<double> tcp_speed_actual_; // Actual speed of the tool given in Cartesian coordinates
|
||||
std::vector<double> tcp_force_; // Generalised forces in the TC
|
||||
std::vector<double> tool_vector_target_; // Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry
|
||||
// and rz is a rotation vector representation of the tool orientation
|
||||
std::vector<double> tcp_speed_target_; // Target speed of the tool given in Cartesian coordinates
|
||||
std::vector<bool> digital_input_bits_; // Current state of the digital inputs. NOTE: these are bits encoded as
|
||||
// int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
|
||||
std::vector<double> motor_temperatures_; // Temperature of each joint in degrees celsius
|
||||
double controller_timer_; // Controller realtime thread execution time
|
||||
double robot_mode_; // Robot mode
|
||||
std::vector<double> joint_modes_; // Joint control modes
|
||||
double safety_mode_; // Safety mode
|
||||
std::vector<double> tool_accelerometer_values_; // Tool x,y and z accelerometer values (software version 1.7)
|
||||
double speed_scaling_; // Speed scaling of the trajectory limiter
|
||||
double linear_momentum_norm_; // Norm of Cartesian linear momentum
|
||||
double v_main_; // Masterboard: Main voltage
|
||||
double v_robot_; // Matorborad: Robot voltage (48V)
|
||||
double i_robot_; // Masterboard: Robot current
|
||||
std::vector<double> v_actual_; // Actual joint voltages
|
||||
|
||||
std::mutex val_lock_; // Locks the variables while unpack parses data;
|
||||
|
||||
std::condition_variable* pMsg_cond_; // Signals that new vars are available
|
||||
bool data_published_; // to avoid spurious wakes
|
||||
bool controller_updated_; // to avoid spurious wakes
|
||||
|
||||
std::vector<double> unpackVector(uint8_t* buf, int start_index, int nr_of_vals);
|
||||
std::vector<bool> unpackDigitalInputBits(int64_t data);
|
||||
double ntohd(uint64_t nf);
|
||||
|
||||
public:
|
||||
RobotStateRT(std::condition_variable& msg_cond);
|
||||
~RobotStateRT();
|
||||
double getVersion();
|
||||
double getTime();
|
||||
std::vector<double> getQTarget();
|
||||
std::vector<double> getQdTarget();
|
||||
std::vector<double> getQddTarget();
|
||||
std::vector<double> getITarget();
|
||||
std::vector<double> getMTarget();
|
||||
std::vector<double> getQActual();
|
||||
std::vector<double> getQdActual();
|
||||
std::vector<double> getIActual();
|
||||
std::vector<double> getIControl();
|
||||
std::vector<double> getToolVectorActual();
|
||||
std::vector<double> getTcpSpeedActual();
|
||||
std::vector<double> getTcpForce();
|
||||
std::vector<double> getToolVectorTarget();
|
||||
std::vector<double> getTcpSpeedTarget();
|
||||
std::vector<bool> getDigitalInputBits();
|
||||
std::vector<double> getMotorTemperatures();
|
||||
double getControllerTimer();
|
||||
double getRobotMode();
|
||||
std::vector<double> getJointModes();
|
||||
double getSafety_mode();
|
||||
std::vector<double> getToolAccelerometerValues();
|
||||
double getSpeedScaling();
|
||||
double getLinearMomentumNorm();
|
||||
double getVMain();
|
||||
double getVRobot();
|
||||
double getIRobot();
|
||||
|
||||
void setVersion(double ver);
|
||||
|
||||
void setDataPublished();
|
||||
bool getDataPublished();
|
||||
bool getControllerUpdated();
|
||||
void setControllerUpdated();
|
||||
std::vector<double> getVActual();
|
||||
void unpack(uint8_t* buf);
|
||||
};
|
||||
|
||||
#endif /* ROBOT_STATE_RT_H_ */
|
||||
@@ -1,53 +0,0 @@
|
||||
#pragma once
|
||||
#include <controller_manager/controller_manager.h>
|
||||
#include <hardware_interface/force_torque_sensor_interface.h>
|
||||
#include <hardware_interface/internal/demangle_symbol.h>
|
||||
#include <hardware_interface/joint_command_interface.h>
|
||||
#include <hardware_interface/joint_state_interface.h>
|
||||
#include <hardware_interface/robot_hw.h>
|
||||
#include "ur_modern_driver/ros/hardware_interface.h"
|
||||
|
||||
using hardware_interface::RobotHW;
|
||||
using hardware_interface::ControllerInfo;
|
||||
|
||||
class RobotHardware : public RobotHW
|
||||
{
|
||||
private:
|
||||
JointInterface joint_interface_;
|
||||
WrenchInterface wrench_interface_;
|
||||
PositionInterface position_interface_;
|
||||
VelocityInterface velocity_interface_;
|
||||
|
||||
HardwareInterface* active_interface_;
|
||||
std::map<std::string, HardwareInterface*> available_interfaces_;
|
||||
|
||||
template <typename T>
|
||||
void registerHardwareInterface(T* interface)
|
||||
{
|
||||
registerInterface<typename T::parent_type>(interface);
|
||||
available_interfaces_[hardware_interface::internal::demangledTypeName<typename T::parent_type>()] = interface;
|
||||
}
|
||||
|
||||
public:
|
||||
RobotHardware(URCommander& commander, std::vector<std::string>& joint_names, double max_vel_change)
|
||||
: joint_interface_(joint_names)
|
||||
, wrench_interface_()
|
||||
, position_interface_(commander, joint_interface_, joint_names)
|
||||
, velocity_interface_(commander, joint_interface_, joint_names, max_vel_change)
|
||||
{
|
||||
registerInterface<hardware_interface::JointStateInterface>(&joint_interface_);
|
||||
registerInterface<hardware_interface::ForceTorqueSensorInterface>(&wrench_interface_);
|
||||
|
||||
registerHardwareInterface(&position_interface_);
|
||||
registerHardwareInterface(&velocity_interface_);
|
||||
}
|
||||
|
||||
// bool canSwitch(const std::list<ControllerInfo>& start_list, const std::list<ControllerInfo>& stop_list) const;
|
||||
void doSwitch(const std::list<ControllerInfo>& start_list, const std::list<ControllerInfo>& stop_list);
|
||||
|
||||
/// \brief Read the state from the robot hardware.
|
||||
virtual void read(RTShared& packet);
|
||||
|
||||
/// \brief write the command to the robot hardware.
|
||||
virtual void write();
|
||||
};
|
||||
@@ -1,63 +0,0 @@
|
||||
/*
|
||||
* ur_communication.h
|
||||
*
|
||||
* Copyright 2015 Thomas Timm Andersen
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef UR_COMMUNICATION_H_
|
||||
#define UR_COMMUNICATION_H_
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <netdb.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#include "do_output.h"
|
||||
#include "robot_state.h"
|
||||
|
||||
class UrCommunication
|
||||
{
|
||||
private:
|
||||
int pri_sockfd_, sec_sockfd_;
|
||||
struct sockaddr_in pri_serv_addr_, sec_serv_addr_;
|
||||
struct hostent* server_;
|
||||
bool keepalive_;
|
||||
std::thread comThread_;
|
||||
int flag_;
|
||||
void run();
|
||||
|
||||
public:
|
||||
bool connected_;
|
||||
RobotState* robot_state_;
|
||||
|
||||
UrCommunication(std::condition_variable& msg_cond, std::string host);
|
||||
bool start();
|
||||
void halt();
|
||||
};
|
||||
|
||||
#endif /* UR_COMMUNICATION_H_ */
|
||||
@@ -1,97 +0,0 @@
|
||||
/*
|
||||
* ur_driver
|
||||
*
|
||||
* Copyright 2015 Thomas Timm Andersen
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef UR_DRIVER_H_
|
||||
#define UR_DRIVER_H_
|
||||
|
||||
#include <math.h>
|
||||
#include <netinet/in.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/types.h>
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include "do_output.h"
|
||||
#include "ur_communication.h"
|
||||
#include "ur_realtime_communication.h"
|
||||
|
||||
#include <chrono>
|
||||
|
||||
class UrDriver
|
||||
{
|
||||
private:
|
||||
double maximum_time_step_;
|
||||
double minimum_payload_;
|
||||
double maximum_payload_;
|
||||
std::vector<std::string> joint_names_;
|
||||
std::string ip_addr_;
|
||||
const int MULT_JOINTSTATE_ = 1000000;
|
||||
const int MULT_TIME_ = 1000000;
|
||||
const unsigned int REVERSE_PORT_;
|
||||
int incoming_sockfd_;
|
||||
int new_sockfd_;
|
||||
bool reverse_connected_;
|
||||
double servoj_time_;
|
||||
bool executing_traj_;
|
||||
double firmware_version_;
|
||||
double servoj_lookahead_time_;
|
||||
double servoj_gain_;
|
||||
|
||||
public:
|
||||
UrRealtimeCommunication* rt_interface_;
|
||||
UrCommunication* sec_interface_;
|
||||
|
||||
UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
|
||||
unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12,
|
||||
double max_time_step = 0.08, double min_payload = 0., double max_payload = 1.,
|
||||
double servoj_lookahead_time = 0.03, double servoj_gain = 300.);
|
||||
bool start();
|
||||
void halt();
|
||||
|
||||
void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
|
||||
|
||||
bool doTraj(std::vector<double> inp_timestamps, std::vector<std::vector<double>> inp_positions,
|
||||
std::vector<std::vector<double>> inp_velocities);
|
||||
void servoj(std::vector<double> positions, int keepalive = 1);
|
||||
|
||||
void stopTraj();
|
||||
|
||||
bool uploadProg();
|
||||
bool openServo();
|
||||
void closeServo(std::vector<double> positions);
|
||||
|
||||
std::vector<double> interp_cubic(double t, double T, std::vector<double> p0_pos, std::vector<double> p1_pos,
|
||||
std::vector<double> p0_vel, std::vector<double> p1_vel);
|
||||
|
||||
std::vector<std::string> getJointNames();
|
||||
void setJointNames(std::vector<std::string> jn);
|
||||
void setToolVoltage(unsigned int v);
|
||||
void setFlag(unsigned int n, bool b);
|
||||
void setDigitalOut(unsigned int n, bool b);
|
||||
void setAnalogOut(unsigned int n, double f);
|
||||
bool setPayload(double m);
|
||||
|
||||
void setMinPayload(double m);
|
||||
void setMaxPayload(double m);
|
||||
void setServojTime(double t);
|
||||
void setServojLookahead(double t);
|
||||
void setServojGain(double g);
|
||||
};
|
||||
|
||||
#endif /* UR_DRIVER_H_ */
|
||||
@@ -1,136 +0,0 @@
|
||||
/*
|
||||
* ur_hardware_control_loop.cpp
|
||||
*
|
||||
* Copyright 2015 Thomas Timm Andersen
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
/* Based on original source from University of Colorado, Boulder. License copied below. */
|
||||
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2015, University of Colorado, Boulder
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Univ of CO, Boulder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************
|
||||
|
||||
Author: Dave Coleman
|
||||
*/
|
||||
|
||||
#ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
|
||||
#define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H
|
||||
|
||||
#include <controller_manager/controller_manager.h>
|
||||
#include <hardware_interface/force_torque_sensor_interface.h>
|
||||
#include <hardware_interface/joint_command_interface.h>
|
||||
#include <hardware_interface/joint_state_interface.h>
|
||||
#include <hardware_interface/robot_hw.h>
|
||||
#include <math.h>
|
||||
#include <ros/ros.h>
|
||||
#include <boost/scoped_ptr.hpp>
|
||||
#include "do_output.h"
|
||||
#include "ur_driver.h"
|
||||
|
||||
namespace ros_control_ur
|
||||
{
|
||||
// For simulation only - determines how fast a trajectory is followed
|
||||
static const double POSITION_STEP_FACTOR = 1;
|
||||
static const double VELOCITY_STEP_FACTOR = 1;
|
||||
|
||||
/// \brief Hardware interface for a robot
|
||||
class UrHardwareInterface : public hardware_interface::RobotHW
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* \brief Constructor
|
||||
* \param nh - Node handle for topics.
|
||||
*/
|
||||
UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot);
|
||||
|
||||
/// \brief Initialize the hardware interface
|
||||
virtual void init();
|
||||
|
||||
/// \brief Read the state from the robot hardware.
|
||||
virtual void read();
|
||||
|
||||
/// \brief write the command to the robot hardware.
|
||||
virtual void write();
|
||||
|
||||
void setMaxVelChange(double inp);
|
||||
|
||||
bool canSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
|
||||
const std::list<hardware_interface::ControllerInfo>& stop_list) const;
|
||||
void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
|
||||
const std::list<hardware_interface::ControllerInfo>& stop_list);
|
||||
|
||||
protected:
|
||||
// Startup and shutdown of the internal node inside a roscpp program
|
||||
ros::NodeHandle nh_;
|
||||
|
||||
// Interfaces
|
||||
hardware_interface::JointStateInterface joint_state_interface_;
|
||||
hardware_interface::ForceTorqueSensorInterface force_torque_interface_;
|
||||
hardware_interface::PositionJointInterface position_joint_interface_;
|
||||
hardware_interface::VelocityJointInterface velocity_joint_interface_;
|
||||
bool velocity_interface_running_;
|
||||
bool position_interface_running_;
|
||||
// Shared memory
|
||||
std::vector<std::string> joint_names_;
|
||||
std::vector<double> joint_position_;
|
||||
std::vector<double> joint_velocity_;
|
||||
std::vector<double> joint_effort_;
|
||||
std::vector<double> joint_position_command_;
|
||||
std::vector<double> joint_velocity_command_;
|
||||
std::vector<double> prev_joint_velocity_command_;
|
||||
std::size_t num_joints_;
|
||||
double robot_force_[3] = { 0., 0., 0. };
|
||||
double robot_torque_[3] = { 0., 0., 0. };
|
||||
|
||||
double max_vel_change_;
|
||||
|
||||
// Robot API
|
||||
UrDriver* robot_;
|
||||
};
|
||||
// class
|
||||
|
||||
} // namespace
|
||||
|
||||
#endif
|
||||
@@ -1,73 +0,0 @@
|
||||
/*
|
||||
* ur_realtime_communication.h
|
||||
*
|
||||
* Copyright 2015 Thomas Timm Andersen
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef UR_REALTIME_COMMUNICATION_H_
|
||||
#define UR_REALTIME_COMMUNICATION_H_
|
||||
|
||||
#include <arpa/inet.h>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <netdb.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
#include <condition_variable>
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#include "do_output.h"
|
||||
#include "robot_state_RT.h"
|
||||
|
||||
class UrRealtimeCommunication
|
||||
{
|
||||
private:
|
||||
unsigned int safety_count_max_;
|
||||
int sockfd_;
|
||||
struct sockaddr_in serv_addr_;
|
||||
struct hostent* server_;
|
||||
std::string local_ip_;
|
||||
bool keepalive_;
|
||||
std::thread comThread_;
|
||||
int flag_;
|
||||
std::recursive_mutex command_string_lock_;
|
||||
std::string command_;
|
||||
unsigned int safety_count_;
|
||||
void run();
|
||||
|
||||
public:
|
||||
bool connected_;
|
||||
RobotStateRT* robot_state_;
|
||||
|
||||
UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host, unsigned int safety_count_max = 12);
|
||||
bool start();
|
||||
void halt();
|
||||
void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.);
|
||||
void addCommandToQueue(std::string inp);
|
||||
void setSafetyCountMax(uint inp);
|
||||
std::string getLocalIp();
|
||||
};
|
||||
|
||||
#endif /* UR_REALTIME_COMMUNICATION_H_ */
|
||||
Reference in New Issue
Block a user