## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
- controller_interface
effort_controllers
- kdl_parser
- geometry_msgs
- nav_msgs
- tf
- message_generation
)
find_package(cmake_modules REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
-find_package(Eigen REQUIRED)
+#find_package(Eigen REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
- INCLUDE_DIRS include
- LIBRARIES ${PROJECT_NAME}
- CATKIN_DEPENDS message_runtime
+#catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES ${PROJECT_NAME}
+# CATKIN_DEPENDS message_runtime
# DEPENDS system_lib
-)
+#)
###########
## Build ##
## Specify additional locations of header files
## Your package locations should be listed before other locations
-include_directories(include
- ${catkin_INCLUDE_DIRS}
- ${Eigen_INCLUDE_DIRS}
-)
+#include_directories(include
+# ${catkin_INCLUDE_DIRS}
+# ${Eigen_INCLUDE_DIRS}
+#)
## Declare a cpp library
-add_library(twil_controllers
- src/cart_linearizing_controller.cpp
-)
+#add_library(twil_controllers
+#)
## Declare a cpp executable
# add_executable(twil_controllers_node src/twil_controllers_node.cpp)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
-add_dependencies(twil_controllers twil_controllers_generate_messages_cpp)
+#add_dependencies(twil_controllers twil_controllers_generate_messages_cpp)
## Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}
- ${catkin_LIBRARIES}
-)
+#target_link_libraries(${PROJECT_NAME}
+# ${catkin_LIBRARIES}
+#)
#############
## Install ##
# )
## Mark executables and/or libraries for installation
-install(TARGETS ${PROJECT_NAME}
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
+#install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+#)
## Mark cpp header files for installation
-install(DIRECTORY include/${PROJECT_NAME}/
- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
-)
+#)
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
rostopic pub /twil/left_wheel_joint_effort_controller/command std_msgs/Float64 "0.0"
-To publish with cart_linearizing_controller:
-
-rostopic pub /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray "data: [0.1, 0.0]"
-
-To publish with nonsmmoth_backstep_controller:
-
-rostopic pub /twil/nonsmooth_backstep_controller/command geometry_ms/Pose2D '{x: 0.0, y: 0.0, theta: 0.0}'
-
To get time and pose:
rostopic echo -p /gazebo/model_states | awk '{FS=","; printf("%g %g %g %g\n",$1/1e9,$11,$12,2*atan2(sqrt($14^2+$15^2+$16^2),$17))}'
+++ /dev/null
-twil:
-
- joint_state_controller:
- type: joint_state_controller/JointStateController
- publish_rate: 100
-
- cart_linearizing_controller:
- type: twil_controllers/CartLinearizingController
- joints:
- - left_wheel_joint
- - right_wheel_joint
+++ /dev/null
-/******************************************************************************
- ROS twil_controllers Package
- Cartesian Linearizing Controller
- Copyright (C) 2014..2017 Walter Fetter Lages <w.fetter@ieee.org>
-
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
-
- This program is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- General Public License for more details.
-
- You should have received a copy of the GNU General Public License
- along with this program. If not, see
- <http://www.gnu.org/licenses/>.
-
-*******************************************************************************/
-
-#ifndef TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H
-#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H
-
-#include <cstddef>
-#include <vector>
-#include <string>
-
-#include <ros/node_handle.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <controller_interface/controller.h>
-#include <std_msgs/Float64MultiArray.h>
-#include <sensor_msgs/JointState.h>
-
-#include <Eigen/Dense>
-
-namespace twil_controllers
-{
- class CartLinearizingController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
- {
- public:
- CartLinearizingController(void);
- ~CartLinearizingController(void);
-
- bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
- void starting(const ros::Time& time);
- void update(const ros::Time& time,const ros::Duration& duration);
-
- private:
- ros::NodeHandle node_;
- hardware_interface::EffortJointInterface *robot_;
- std::vector<hardware_interface::JointHandle> joints_;
-
- void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command);
- ros::Subscriber sub_command_;
-
- Eigen::Vector2d nu;
- Eigen::Vector2d torque;
- Eigen::Vector2d v;
- Eigen::Vector2d u;
-
- Eigen::Matrix2d Ginv;
- Eigen::Matrix2d F;
-
- std::vector<double> wheelRadius;
- double wheelBase;
- };
-}
-#endif
+++ /dev/null
-<launch>
- <rosparam file="$(find twil_controllers)/config/linearizing_control.yaml" command="load"/>
-
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="twil" args="joint_state_controller cart_linearizing_controller"/>
-</launch>
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
- <build_depend>controller_interface</build_depend>
<build_depend>effort_controllers</build_depend>
- <build_depend>kdl_parser</build_depend>
- <build_depend>geometry_msgs</build_depend>
- <build_depend>nav_msgs</build_depend>
- <build_depend>tf</build_depend>
- <build_depend>control_toolbox</build_depend>
- <build_depend>realtime_tools</build_depend>
- <build_depend>message_generation</build_depend>
- <run_depend>controller_interface</run_depend>
<run_depend>controller_manager</run_depend>
<run_depend>effort_controllers</run_depend>
<run_depend>joint_state_controller</run_depend>
- <run_depend>kdl_parser</run_depend>
- <run_depend>geometry_msgs</run_depend>
- <run_depend>nav_msgs</run_depend>
- <run_depend>tf</run_depend>
- <run_depend>control_toolbox</run_depend>
- <run_depend>realtime_tools</run_depend>
- <run_depend>message_runtime</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
- <controller_interface plugin="${prefix}/twil_controllers_plugins.xml"/>
</export>
</package>
+++ /dev/null
-/******************************************************************************
- ROS twil_controllers Package
- Cartesian Linearizing Controller
- Copyright (C) 2014..2017 Walter Fetter Lages <w.fetter@ieee.org>
-
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
-
- This program is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- General Public License for more details.
-
- You should have received a copy of the GNU General Public License
- along with this program. If not, see
- <http://www.gnu.org/licenses/>.
-
-*******************************************************************************/
-
-#include <twil_controllers/cart_linearizing_controller.h>
-#include <pluginlib/class_list_macros.h>
-
-#include <kdl/frames.hpp>
-#include <kdl_parser/kdl_parser.hpp>
-
-namespace twil_controllers
-{
- CartLinearizingController::CartLinearizingController(void):
- wheelRadius(2)
- {
- }
-
- CartLinearizingController::~CartLinearizingController(void)
- {
- sub_command_.shutdown();
- }
-
- bool CartLinearizingController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
- {
- node_=n;
- robot_=robot;
-
- XmlRpc::XmlRpcValue joint_names;
- if(!node_.getParam("joints",joint_names))
- {
- ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
- {
- ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- for(int i=0; i < joint_names.size();i++)
- {
- XmlRpc::XmlRpcValue &name_value=joint_names[i];
- if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
- {
- ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- hardware_interface::JointHandle j=robot->getHandle((std::string)name_value);
- joints_.push_back(j);
- }
- sub_command_ = node_.subscribe("command",1000,&CartLinearizingController::commandCB,this);
-
- std::string robot_desc_string;
- if(!node_.getParam("/robot_description",robot_desc_string))
- {
- ROS_ERROR("Could not find '/robot_description'.");
- return false;
- }
-
- KDL::Tree tree;
- if (!kdl_parser::treeFromString(robot_desc_string,tree))
- {
- ROS_ERROR("Failed to construct KDL tree.");
- return false;
- }
-
- // get wheelBase from URDF (actually from KDL tree)
- KDL::SegmentMap::const_iterator segmentMapIter=tree.getSegment("left_wheel_support");
- KDL::Frame leftSupportFrame=segmentMapIter->second.segment.getFrameToTip();
-
- segmentMapIter=tree.getSegment("right_wheel_support");
- KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip();
-
- wheelRadius.resize(joints_.size());
- wheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3);
-
- // get wheelRadius from URDF (actually from KDL tree)
- segmentMapIter=tree.getSegment("chassis");
- KDL::Frame chassisFrame=segmentMapIter->second.segment.getFrameToTip();
-
- segmentMapIter=tree.getSegment("left_wheel");
- KDL::Joint leftWheelJoint=segmentMapIter->second.segment.getJoint();
- wheelRadius[0]=chassisFrame(2,3)+leftSupportFrame(2,3)+leftWheelJoint.JointOrigin().z();
-
- segmentMapIter=tree.getSegment("right_wheel");
- KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint();
- wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z();
-
- const double K5 = 0.0018533548425194695;
- const double K6 = 0.09946140462774823;
- const double K7 = 21.65458426501294;
- const double K8 = -15.40102896939387;
- Ginv << 0.5*K7, 0.5*K8,
- 0.5*K7, -0.5*K8;
- F << 0.0, K5,
- K6, 0.0;
-
- return true;
- }
-
- void CartLinearizingController::starting(const ros::Time& time)
- {
- v.setZero();
- }
-
- void CartLinearizingController::update(const ros::Time& time,
- const ros::Duration& duration)
- {
-
- for(unsigned int i=0;i < joints_.size();i++)
- {
- nu[i]=joints_[i].getVelocity();
- }
-
- u[0]=(nu[0]*wheelRadius[0]+nu[1]*wheelRadius[1])/2.0;
- u[1]=(nu[0]*wheelRadius[0]-nu[1]*wheelRadius[1])/wheelBase;
-
- // Compute linearization
- Eigen::Vector2d uf;
- uf << u[0]*u[1], u[1]*u[1];
-
- torque=Ginv*(v+F*uf);
-
- // Apply torques
- for(unsigned int i=0;i < joints_.size();i++) joints_[i].setCommand(torque(i));
- }
-
- void CartLinearizingController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command)
- {
- for(unsigned int i=0;i < command->data.size();i++) v[i]=command->data[i];
- }
-}
-PLUGINLIB_EXPORT_CLASS(twil_controllers::CartLinearizingController,controller_interface::ControllerBase)
+++ /dev/null
-<library path="lib/libtwil_controllers">
-
- <class name="twil_controllers/CartLinearizingController" type="twil_controllers::CartLinearizingController" base_class_type="controller_interface::ControllerBase">
- <description>
- The CartLinearizingController linearizes the Twil dynamic model. The
- linearized inputs are linear and angular accelerations. It expects a
- EffortJointInterface type of hardware interface.
- </description>
- </class>
-
-</library>
projects / twil.git / commitdiff