Skip to content

Commit bf015ea

Browse files
civerachb-cprciverachb-cpr
committed
Add the 3-finger gripper packages from husky_UR_accessories. Do some minor refactoring so the packages follow the same format as the existing 2f models
1 parent 3a1dbd8 commit bf015ea

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

41 files changed

+4444
-0
lines changed

‎robotiq_3f_gripper_control/CMakeLists.txt

+56
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
2+
cmake_minimum_required(VERSION 2.8.3)
3+
project(robotiq_3f_gripper_control)
4+
find_package(catkin REQUIRED COMPONENTS rospy message_generation roscpp robotiq_ethercat robotiq_3f_gripper_msgs)
5+
6+
#set the default path for built executables to the "bin" directory
7+
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
8+
#set the default path for built libraries to the "lib" directory
9+
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
10+
11+
###################################
12+
## catkin message generation ##
13+
###################################
14+
15+
catkin_python_setup()
16+
17+
###################################
18+
## catkin specific configuration ##
19+
###################################
20+
## The catkin_package macro generates cmake config files for your package
21+
## Declare things to be passed to dependent projects
22+
## LIBRARIES: libraries you create in this project that dependent projects also need
23+
## CATKIN_DEPENDS: catkin_packages dependent projects also need
24+
## DEPENDS: system dependencies of this project that dependent projects also need
25+
catkin_package(
26+
CATKIN_DEPENDS rospy message_runtime roscpp robotiq_ethercat
27+
)
28+
29+
include_directories(include)
30+
include_directories(
31+
${catkin_INCLUDE_DIRS}
32+
${robotiq_ethercat_INCLUDE_DIRS}
33+
${robotiq_3f_gripper_msgs_INCLUDE_DIRS}
34+
)
35+
36+
add_executable(3f_gripper_node
37+
src/robotiq_s_model_control/s_model_ethercat_node.cpp
38+
src/robotiq_s_model_control/s_model_ethercat_client.cpp
39+
)
40+
41+
add_dependencies(3f_gripper_node ${robotiq_3f_gripper_control_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${robotiq_3f_gripper_msgs_EXPORTED_TARGETS})
42+
43+
target_link_libraries(3f_gripper_node
44+
${robotiq_ethercat_LIBRARIES}
45+
${catkin_LIBRARIES}
46+
)
47+
48+
#############
49+
## Install ##
50+
#############
51+
52+
# all install targets should use catkin DESTINATION variables
53+
# See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html
54+
55+
install(PROGRAMS nodes/SModelSimpleController.py nodes/SModelStatusListener.py
56+
nodes/SModelTcpNode.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

‎robotiq_3f_gripper_control/cfg/SModel.cfg

+47
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
#!/usr/bin/env python
2+
3+
# Copyright (c) 2016, Toyota Research Institute. All rights reserved.
4+
5+
# Redistribution and use in source and binary forms, with or without
6+
# modification, are permitted provided that the following conditions are met:
7+
8+
# 1. Redistributions of source code must retain the above copyright
9+
# notice, this list of conditions and the following disclaimer.
10+
11+
# 2. Redistributions in binary form must reproduce the above copyright
12+
# notice, this list of conditions and the following disclaimer in the
13+
# documentation and/or other materials provided with the distribution.
14+
15+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16+
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17+
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18+
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
19+
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20+
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21+
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22+
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23+
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24+
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25+
# POSSIBILITY OF SUCH DAMAGE.
26+
27+
28+
PACKAGE="robotiq_3f_gripper_control"
29+
30+
from dynamic_reconfigure.parameter_generator_catkin import *
31+
32+
gen = ParameterGenerator()
33+
34+
gen.add("ind_control_fingers", bool_t, 0, "Set individual control of fingers", False)
35+
gen.add("ind_control_scissor", bool_t, 0, "Set individual control of scissor", False)
36+
37+
mode_enum = gen.enum([gen.const("Basic", int_t, 0, "Basic mode"),
38+
gen.const("Pinch", int_t, 1, "Pinch mode"),
39+
gen.const("Wide", int_t, 2, "Wide mode"),
40+
gen.const("Scissor", int_t, 3, "Scissor mode")], "An enum to set the grasp operation mode")
41+
42+
gen.add("mode", int_t, 0, "The grasping mode", 0, edit_method = mode_enum)
43+
44+
gen.add("velocity", double_t, 0, "Set velocity for fingers", 66, 22, 110)
45+
gen.add("force", double_t, 0, "Set force for fingers", 30, 15, 60)
46+
47+
exit(gen.generate(PACKAGE, "robotiq_3f_gripper_control", "SModel"))

‎robotiq_3f_gripper_control/include/robotiq_3f_gripper_control/s_model_api.h

+115
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
// Copyright (c) 2016, Toyota Research Institute. All rights reserved.
2+
3+
// Redistribution and use in source and binary forms, with or without
4+
// modification, are permitted provided that the following conditions are met:
5+
6+
// 1. Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
9+
// 2. Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
12+
13+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
14+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
15+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
16+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
17+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
19+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
20+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
21+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
22+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
23+
// POSSIBILITY OF SUCH DAMAGE.
24+
25+
#ifndef S_MODEL_API_H
26+
#define S_MODEL_API_H
27+
28+
#include <robotiq_3f_gripper_control/s_model_client_base.h>
29+
30+
namespace robotiq
31+
{
32+
enum InitializationMode { INIT_RESET, INIT_ACTIVATION };
33+
enum GraspingMode { GRASP_BASIC, GRASP_PINCH, GRASP_WIDE, GRASP_SCISSOR };
34+
enum ActionMode { ACTION_STOP, ACTION_GO };
35+
enum GripperStatus { GRIPPER_RESET, GRIPPER_ACTIVATING, GRIPPER_MODE_CHANGE, GRIPPER_READY };
36+
enum MotionStatus { MOTION_STARTED, MOTION_PARTIAL_STOP, MOTION_ALL_STOP, MOTION_COMPLETE };
37+
enum ObjectStatus { OBJECT_MOTION, OBJECT_OPEN_CONTACT, OBJECT_CLOSE_CONTACT, OBJECT_MOTION_COMPLETE };
38+
enum FaultStatus { FAULT_NONE, FAULT_UNKNOWN_1, FAULT_UNKNOWN_2, FAULT_UNKNOWN_3,
39+
NOTICE, NOTICE_ACTIVATION_DELAYED, NOTICE_MODE_DELAYED, NOTICE_ACTIVATION_NEEDED,
40+
WARNING, WARNING_COMM_NOT_READY, WARNING_MODE, WARNING_AUTOMATIC_RELEASE,
41+
ERROR, ERROR_ACTIVATION_FAULT, ERROR_MODE_FAULT, ERROR_AUTOMATIC_RELEASE_COMPLETE };
42+
43+
enum EmergencyRelease { EMERGENCY_RELEASE_IDLE, EMERGENCY_RELEASE_ENGAGED };
44+
enum IndividualControl { IND_CONTROL_OFF, IND_CONTROL_ON };
45+
} // end namespace robotiq
46+
47+
namespace robotiq_3f_gripper_control
48+
{
49+
using namespace robotiq;
50+
51+
class SModelAPI
52+
{
53+
public:
54+
SModelAPI(boost::shared_ptr<SModelClientBase> base);
55+
56+
void setInitialization(InitializationMode mode);
57+
void setGraspingMode(GraspingMode mode);
58+
void setActionMode(ActionMode mode);
59+
void setEmergencyRelease(EmergencyRelease release);
60+
void setInidividualControlMode(IndividualControl fingers, IndividualControl scissor);
61+
void setPosition(const double &posA, const double &posB=0, const double &posC=0, const double &posS=0);
62+
void setVelocity(const double &velA, const double &velB=0, const double &velC=0, const double &velS=0);
63+
void setForce(const double &fA, const double &fB=0, const double &fC=0, const double &fS=0);
64+
void setRaw(const SModelClientBase::GripperOutput &raw);
65+
66+
void getPosition(double *posA, double *posB, double *posC, double *posS) const;
67+
void getPositionCmd(double *posA, double *posB, double *posC, double *posS) const;
68+
void getCurrent(double *curA, double *curB, double *curC, double *curS) const;
69+
void getGripperStatus(InitializationMode *gACT, GraspingMode *gMOD, ActionMode *gGTO, GripperStatus *gIMC, MotionStatus *gSTA) const;
70+
void getFaultStatus(FaultStatus *gFLT) const;
71+
void getObjectStatus(ObjectStatus *fA, ObjectStatus *fB, ObjectStatus *fC, ObjectStatus *fS) const;
72+
void getRaw(SModelClientBase::GripperInput *raw) const;
73+
74+
void getCommandPos(double *posA, double *posB, double *posC, double *posS) const;
75+
76+
bool isInitialized();
77+
bool isReady();
78+
bool isModeSet(GraspingMode mode);
79+
bool isHalted();
80+
bool isMoving();
81+
bool isEmergReleaseComplete();
82+
83+
void read();
84+
void write();
85+
86+
private:
87+
boost::shared_ptr<SModelClientBase> base_;
88+
89+
SModelClientBase::GripperInput status_;
90+
SModelClientBase::GripperOutput command_;
91+
92+
//! conversions
93+
double pos_to_ticks_;
94+
double pos_offset_;
95+
double sci_to_ticks_;
96+
double sci_offset_;
97+
double vel_to_ticks_;
98+
double vel_offset_;
99+
double force_to_ticks_;
100+
double force_offset_;
101+
double cur_to_ticks_;
102+
103+
};
104+
105+
template <typename T>
106+
inline T limit (double value)
107+
{
108+
value = value < std::numeric_limits<T>::min() ? std::numeric_limits<T>::min() : value;
109+
value = value > std::numeric_limits<T>::max() ? std::numeric_limits<T>::max() : value;
110+
return static_cast<T>(value);
111+
}
112+
113+
} //end namespace robotiq_3f_gripper_control
114+
115+
#endif // S_MODEL_API_H

‎robotiq_3f_gripper_control/include/robotiq_3f_gripper_control/s_model_can_client.h

+110
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
// Copyright (c) 2016, Toyota Research Institute. All rights reserved.
2+
3+
// Redistribution and use in source and binary forms, with or without
4+
// modification, are permitted provided that the following conditions are met:
5+
6+
// 1. Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
9+
// 2. Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
12+
13+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
14+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
15+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
16+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
17+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
19+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
20+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
21+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
22+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
23+
// POSSIBILITY OF SUCH DAMAGE.
24+
25+
#ifndef S_MODEL_CAN_CLIENT_H
26+
#define S_MODEL_CAN_CLIENT_H
27+
28+
#include <robotiq_3f_gripper_control/s_model_client_base.h>
29+
#include <socketcan_interface/socketcan.h>
30+
#include <socketcan_interface/threading.h>
31+
32+
namespace robotiq_3f_gripper_control
33+
{
34+
35+
/**
36+
* \brief This class provides a client for the EtherCAT manager object that
37+
* can translate robot input/output messages and translate them to
38+
* the underlying IO Map.
39+
*/
40+
41+
class SModelCanClient : public SModelClientBase
42+
{
43+
public:
44+
SModelCanClient(unsigned int can_id, boost::shared_ptr<can::DriverInterface> driver);
45+
46+
virtual ~SModelCanClient();
47+
48+
49+
void init(ros::NodeHandle nh);
50+
51+
/**
52+
* \brief Write the given set of control flags to the memory of the gripper
53+
*
54+
* @param[in] output The set of output-register values to write to the gripper
55+
*/
56+
void writeOutputs(const GripperOutput& output);
57+
58+
/**
59+
* \brief Reads set of input-register values from the gripper.
60+
* \return The gripper input registers as read from the controller IOMap
61+
*/
62+
GripperInput readInputs() const;
63+
64+
/**
65+
* \brief Reads set of output-register values from the gripper.
66+
* \return The gripper output registers as read from the controller IOMap
67+
*/
68+
GripperOutput readOutputs() const;
69+
70+
private:
71+
unsigned int can_id_;
72+
boost::shared_ptr<can::DriverInterface> driver_;
73+
std::string can_device_;
74+
can::Frame::Header resp_header_;
75+
76+
GripperInput input_;
77+
GripperOutput output_;
78+
79+
can::CommInterface::FrameListener::Ptr frame_listener_;
80+
can::StateInterface::StateListener::Ptr state_listener_;
81+
82+
void frameCallback(const can::Frame &f);
83+
void stateCallback(const can::State &s);
84+
85+
void requestStart();
86+
87+
std::map<unsigned char, unsigned char> prevCmd_;
88+
89+
mutable boost::timed_mutex read_mutex;
90+
91+
void decodeGripperStatus(const u_int8_t &f);
92+
void decodeObjectStatus(const u_int8_t &f);
93+
void decodeFaultStatus(const u_int8_t &f);
94+
void decodeFingerAPos(const u_int8_t &f);
95+
void decodeFingerBPos(const u_int8_t &f);
96+
void decodeFingerCPos(const u_int8_t &f);
97+
void decodeFingerSPos(const u_int8_t &f);
98+
void decodeFingerACurrent(const u_int8_t &f);
99+
void decodeFingerBCurrent(const u_int8_t &f);
100+
void decodeFingerCCurrent(const u_int8_t &f);
101+
void decodeFingerSCurrent(const u_int8_t &f);
102+
void decodeFingerAPosCmd(const u_int8_t &f);
103+
void decodeFingerBPosCmd(const u_int8_t &f);
104+
void decodeFingerCPosCmd(const u_int8_t &f);
105+
void decodeFingerSPosCmd(const u_int8_t &f);
106+
};
107+
108+
} //end namespace robotiq_3f_gripper_control
109+
110+
#endif // S_MODEL_CAN_CLIENT_H

0 commit comments

Comments
 (0)