Skip to content
Snippets Groups Projects
Commit c244698c authored by Erdal Pekel's avatar Erdal Pekel
Browse files

Migrate to Position based control, add custom controller for invalid configuration spawns.

parent cde500f9
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 55
− 50
View file @ c244698c
cmake_minimum_required(VERSION 2.8.3) cmake_minimum_required(VERSION 2.8.3)
project(panda_simulation) project(panda_simulation)
## Compile as C++11, supported in ROS Kinetic and newer # Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++14) add_compile_options(-std=c++14)
set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive") set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}" ) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")
## Find catkin macros and libraries # Find catkin macros and libraries if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) is used, also find other
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) # catkin packages
## is used, also find other catkin packages find_package(catkin REQUIRED
find_package(catkin REQUIRED COMPONENTS COMPONENTS controller_manager
controller_manager effort_controllers
effort_controllers gazebo_ros
gazebo_ros joint_state_controller
joint_state_controller joint_state_publisher
joint_state_publisher joint_trajectory_controller
joint_trajectory_controller robot_state_publisher
robot_state_publisher roscpp
roscpp std_msgs
std_msgs tf
tf xacro
xacro moveit_core
moveit_core moveit_visual_tools
moveit_visual_tools moveit_ros_planning
moveit_ros_planning moveit_ros_planning_interface
moveit_ros_planning_interface controller_interface
) hardware_interface)
## System dependencies are found with CMake's conventions # System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS filesystem) find_package(Boost REQUIRED COMPONENTS filesystem)
find_package(PkgConfig REQUIRED) find_package(PkgConfig REQUIRED)
pkg_check_modules(JSONCPP jsoncpp) pkg_check_modules(JSONCPP jsoncpp)
message(${JSONCPP_LIBRARIES}) message(${JSONCPP_LIBRARIES})
################################### # ################################################################################################################################
## catkin specific configuration ## # catkin specific configuration ##
################################### # ################################################################################################################################
## The catkin_package macro generates cmake config files for your package # The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
## Declare things to be passed to dependent projects catkin_package(CATKIN_DEPENDS
catkin_package( moveit_core
CATKIN_DEPENDS moveit_visual_tools
moveit_core moveit_ros_planning_interface
moveit_visual_tools controller_interface
moveit_ros_planning_interface hardware_interface
) pluginlib
DEPENDS
########### system_lib)
## Build ##
########### # ################################################################################################################################
# Build ##
## Specify additional locations of header files # ################################################################################################################################
## Your package locations should be listed before other locations
include_directories( # Specify additional locations of header files Your package locations should be listed before other locations
${catkin_INCLUDE_DIRS} include_directories(${catkin_INCLUDE_DIRS})
)
add_executable(robot_control_node src/robot_control_node.cpp)
add_executable(robot_control_node src/robot_control_node.cpp) add_executable(box_publisher_node src/box_publisher.cpp)
add_executable(box_publisher_node src/box_publisher.cpp)
target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES}) target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
target_link_libraries(box_publisher_node ${catkin_LIBRARIES}) target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
\ No newline at end of file
# add custom controller as library
add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
# Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
config/panda_control.yaml
+ 19
− 18
View file @ c244698c
# Publish joint states # Publish joint states
joint_state_controller: joint_state_controller:
type: joint_state_controller/JointStateController type: joint_state_controller/JointStateController
publish_rate: 50 publish_rate: 100
panda_arm_controller: panda_arm_controller:
type: effort_controllers/JointTrajectoryController type: position_controllers/JointTrajectoryController
joints: joints:
- panda_joint1 - panda_joint1
- panda_joint2 - panda_joint2
...@@ -14,28 +14,29 @@ ...@@ -14,28 +14,29 @@
- panda_joint6 - panda_joint6
- panda_joint7 - panda_joint7
gains:
panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }
constraints: constraints:
goal_time: 2.0 goal_time: 2.0
state_publish_rate: 25 state_publish_rate: 100
panda_hand_controller: panda_hand_controller:
type: effort_controllers/JointTrajectoryController type: position_controllers/JointTrajectoryController
joints: joints:
- panda_finger_joint1 - panda_finger_joint1
- panda_finger_joint2 - panda_finger_joint2
gains: state_publish_rate: 25
panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 } joint_position_controller:
panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 } type: panda_simulation/JointPositionController
arm_id: panda
state_publish_rate: 25 joint_names:
\ No newline at end of file - panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
gains: [
1, 1, 1, 1, 1, 1, 1
]
\ No newline at end of file
controller_plugins.xml 0 → 100644
+ 3
− 0
View file @ c244698c
<library path="lib/libpanda_simulation_controllers_lib">
<class name="panda_simulation/JointPositionController" type="panda_simulation::JointPositionController" base_class_type="controller_interface::ControllerBase" />
</library>
\ No newline at end of file
package.xml
+ 7
− 4
View file @ c244698c
...@@ -17,7 +17,7 @@ ...@@ -17,7 +17,7 @@
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>catkin</buildtool_depend>
<depend>controller_manager</depend> <depend>controller_manager</depend>
<depend>effort_controllers</depend> <depend>effort_controllers</depend>
<depend>gazebo_ros</depend> <depend>gazebo_ros</depend>
...@@ -32,16 +32,19 @@ ...@@ -32,16 +32,19 @@
<depend>xacro</depend> <depend>xacro</depend>
<depend>libjsoncpp-dev</depend> <depend>libjsoncpp-dev</depend>
<depend>moveit_simple_controller_manager</depend> <depend>moveit_simple_controller_manager</depend>
<!-- CUSTOM --> <!-- CUSTOM -->
<depend>moveit_core</depend> <depend>moveit_core</depend>
<depend>moveit_ros_planning_interface</depend> <depend>moveit_ros_planning_interface</depend>
<depend>moveit_ros_perception</depend> <depend>moveit_ros_perception</depend>
<depend>moveit_visual_tools</depend> <depend>moveit_visual_tools</depend>
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<exec_depend>pluginlib</exec_depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->
<export> <export>
<!-- Other tools can request additional information be placed here --> <controller_interface plugin="${prefix}/controller_plugins.xml" />
</export> </export>
</package> </package>
src/joint_position_controller.cpp 0 → 100644
+ 62
− 0
View file @ c244698c
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>
#include <vector>
namespace panda_simulation {
class JointPositionController : public controller_interface::Controller<hardware_interface::PositionJointInterface> {
bool init(hardware_interface::PositionJointInterface *hw, ros::NodeHandle &n) {
std::vector<std::string> joint_names;
if (!n.getParam("joint_names", joint_names)) {
ROS_ERROR("Could not read joint names from param server");
return false;
}
// retrieve gains
if (!n.getParam("gains", gains_vec_)) {
ROS_ERROR("Could not read joint gains from param server");
return false;
}
for (auto &joint_name : joint_names) {
joint_handles_.push_back(hw->getHandle(joint_name));
}
for (auto &joint_handle : joint_handles_) {
command_.push_back(joint_handle.getPosition());
}
sub_command_ = n.subscribe<std_msgs::Float64MultiArray>(std::string("command"), 1,
&JointPositionController::setCommandCallback, this);
return true;
}
void update(const ros::Time &time, const ros::Duration &period) {
for (size_t i = 0; i < joint_handles_.size(); i++) {
double error = command_.at(i) - joint_handles_.at(i).getPosition();
double commanded_effort = error * gains_vec_.at(i);
joint_handles_.at(i).setCommand(commanded_effort);
}
}
void setCommandCallback(const std_msgs::Float64MultiArrayConstPtr &msg) { command_ = msg->data; }
void starting(const ros::Time &time) {}
void stopping(const ros::Time &time) {}
private:
std::vector<hardware_interface::JointHandle> joint_handles_;
std::vector<double> gains_vec_;
std::vector<double> command_;
ros::Subscriber sub_command_;
};
PLUGINLIB_EXPORT_CLASS(panda_simulation::JointPositionController, controller_interface::ControllerBase);
} // namespace panda_simulation
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment