#common commands for building c++ executables and libraries
rosbuild_add_library(${PROJECT_NAME} src/cart_linearizing_controller.cpp)
+rosbuild_add_library(${PROJECT_NAME} src/cart_linearizing_controller_rne.cpp)
+rosbuild_add_library(${PROJECT_NAME} src/nonsmooth_backstep_controller.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
+
+find_package(Eigen REQUIRED)
+include_directories(${Eigen_INCLUDE_DIRS})
-To publish v:
+To publish with joint_effort_controller:
-rostopic pub /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray "data: [0.0, 0.04]"
+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 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
+
+ nonsmooth_backstep_controller:
+ type: twil_controllers/NonSmoothBackstepController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/JointState.h>
-#include <kdl/frames.hpp>
-#include <kdl_parser/kdl_parser.hpp>
-#include <kdl/chainidsolver_recursive_newton_euler.hpp>
-
+#include <Eigen/Dense>
namespace twil_controllers
{
void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command);
ros::Subscriber sub_command_;
- KDL::ChainIdSolver_RNE *idsolver;
-
- KDL::JntArray phi;
- KDL::JntArray nu;
- KDL::JntArray dnu;
- KDL::JntArray torque;
- std::vector<double> v;
-
- KDL::Wrenches fext;
+ Eigen::Vector2d nu;
+ Eigen::Vector2d torque;
+ Eigen::Vector2d v;
+ Eigen::Vector2d u;
+ Eigen::Matrix2d Ginv;
+ Eigen::Matrix2d F;
+
std::vector<double> wheelRadius;
double wheelBase;
};
--- /dev/null
+#ifndef TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_RNE_H
+#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_RNE_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 <kdl/frames.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+
+
+namespace twil_controllers
+{
+ class CartLinearizingController_RNE: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+ {
+ public:
+ CartLinearizingController_RNE(void);
+ ~CartLinearizingController_RNE(void);
+
+ bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
+ void starting(const ros::Time& time);
+ void update(const ros::Time& time);
+
+ private:
+ ros::NodeHandle node_;
+ ros::Time last_time_;
+ hardware_interface::EffortJointInterface *robot_;
+ std::vector<hardware_interface::JointHandle> joints_;
+
+ void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command);
+ ros::Subscriber sub_command_;
+
+ KDL::ChainIdSolver_RNE *idsolver;
+
+ KDL::JntArray phi;
+ KDL::JntArray nu;
+ KDL::JntArray dnu;
+ KDL::JntArray torque;
+ std::vector<double> v;
+
+ KDL::Wrenches fext;
+
+ std::vector<double> wheelRadius;
+ double wheelBase;
+ };
+}
+#endif
--- /dev/null
+#ifndef TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H
+#define TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_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 NonSmoothBackstepController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+ {
+ public:
+ NonSmoothBackstepController(void);
+ ~NonSmoothBackstepController(void);
+
+ bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
+ void starting(const ros::Time& time);
+ void update(const ros::Time& time);
+
+ private:
+ ros::NodeHandle node_;
+ ros::Time last_time_;
+ hardware_interface::EffortJointInterface *robot_;
+ std::vector<hardware_interface::JointHandle> joints_;
+
+ ros::Subscriber sub_command_;
+ ros::Subscriber sub_parameters_;
+
+ Eigen::Matrix2d Ginv;
+ Eigen::Matrix2d F;
+
+ std::vector<double> wheelRadius;
+ double wheelBase;
+
+ Eigen::Vector3d xi;
+ Eigen::Vector3d xiRef;
+
+ Eigen::Vector2d eta;
+
+ void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command);
+ void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command);
+ };
+}
+#endif
--- /dev/null
+<launch>
+ <remap from="/twil/nonsmooth_backstep_controller/dynamic_parameters" to="/twil/dynamic_parameters"/>
+
+ <arg name="paused" default="true"/>
+
+ <include file="$(find twil_description)/launch/twil_sim.launch">
+ <arg name="paused" value="$(arg paused)"/>
+ </include>
+
+ <rosparam file="$(find twil_controllers)/config/nonsmooth_backstep_control.yaml" command="load"/>
+
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="/twil" args="joint_state_controller nonsmooth_backstep_controller"/>
+
+ <node name="ident" ns="/twil" pkg="twil_ident" type="ident" output="screen">
+ <remap from="ident/dynamic_parameters" to="dynamic_parameters"/>
+ </node>
+
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
+</launch>
<launch>
- <include file="$(find twil_description)/launch/twil_sim.launch"/>
+ <arg name="paused" default="true"/>
+
+ <include file="$(find twil_description)/launch/twil_sim.launch">
+ <arg name="paused" value="$(arg paused)"/>
+ </include>
<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"/>
- <!--node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /-->
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
</launch>
<launch>
- <include file="$(find twil_description)/launch/twil_sim.launch"/>
+ <arg name="paused" default="true"/>
+
+ <include file="$(find twil_description)/launch/twil_sim.launch">
+ <arg name="paused" value="$(arg paused)"/>
+ </include>
<rosparam file="$(find twil_controllers)/config/effort_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/twil" args="left_wheel_joint_effort_controller right_wheel_joint_effort_controller joint_state_controller"/>
+
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
</launch>
<launch>
- <include file="$(find twil_description)/launch/twil_sim.launch"/>
+ <arg name="paused" default="true"/>
+
+ <include file="$(find twil_description)/launch/twil_sim.launch">
+ <arg name="paused" value="$(arg paused)"/>
+ </include>
<rosparam file="$(find twil_controllers)/config/velocity_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/twil" args="joint_state_controller left_wheel_joint_velocity_controller right_wheel_joint_velocity_controller"/>
+
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
</launch>
--- /dev/null
+<launch>
+
+ <arg name="paused" default="true"/>
+
+ <include file="$(find twil_description)/launch/twil_sim.launch">
+ <arg name="paused" value="$(arg paused)"/>
+ </include>
+
+ <rosparam file="$(find twil_controllers)/config/nonsmooth_backstep_control.yaml" command="load"/>
+
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="/twil" args="joint_state_controller nonsmooth_backstep_controller"/>
+
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
+</launch>
--- /dev/null
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b twil_controllers
+
+<!--
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/
#include <kdl/frames.hpp>
#include <kdl_parser/kdl_parser.hpp>
-#include <kdl/chainidsolver_recursive_newton_euler.hpp>
namespace twil_controllers
{
CartLinearizingController::CartLinearizingController(void):
- phi(0),nu(0),dnu(0),torque(0),v(0),fext(0),wheelRadius(0)
+ wheelRadius(2)
{
}
hardware_interface::JointHandle j=robot->getJointHandle((std::string)name_value);
joints_.push_back(j);
- v.push_back(0);
}
- sub_command_ = node_.subscribe("command",1000,&CartLinearizingController::commandCB, this);
+ sub_command_ = node_.subscribe("command",1000,&CartLinearizingController::commandCB,this);
std::string robot_desc_string;
if(!node_.getParam("/robot_description",robot_desc_string))
return false;
}
- KDL::Chain chain;
- if (!tree.getChain("left_wheel","right_wheel",chain))
- {
- ROS_ERROR("Failed to get chain from KDL tree.");
- return false;
- }
-
- // Get gravity from gazebo or set values if not simulating
- KDL::Vector g;
- node_.param("/gazebo/gravity_x",g[1],0.0);
- node_.param("/gazebo/gravity_y",g[1],0.0);
- node_.param("/gazebo/gravity_z",g[2],-9.8);
-
- if((idsolver=new KDL::ChainIdSolver_RNE(chain,g)) == NULL)
- {
- ROS_ERROR("Failed to create ChainIDSolver_RNE.");
- 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(chain.getNrOfJoints());
+ wheelRadius.resize(joints_.size());
wheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3);
// get wheelRadius from URDF (actually from KDL tree)
KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint();
wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z();
- // set vectors to the right size
- phi.resize(chain.getNrOfJoints());
- nu.resize(chain.getNrOfJoints());
- dnu.resize(chain.getNrOfJoints());
- torque.resize(chain.getNrOfJoints());
+ 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;
- fext.resize(chain.getNrOfSegments());
-
return true;
}
void CartLinearizingController::starting(const ros::Time& time)
{
last_time_=time;
- for(unsigned int i=0; i < joints_.size();i++) v[i]=0.0;
+ v.setZero();
}
void CartLinearizingController::update(const ros::Time& time)
for(unsigned int i=0;i < joints_.size();i++)
{
- phi(i)=joints_[i].getPosition();
- nu(i)=joints_[i].getVelocity();
+ 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;
- dnu(0)=v[0]/wheelRadius[0]-v[1]*wheelBase/2.0/wheelRadius[0]; // left wheel
- dnu(1)=v[0]/wheelRadius[1]+v[1]*wheelBase/2.0/wheelRadius[1]; // right wheel
+ // Compute linearization
+ Eigen::Vector2d uf;
+ uf << u[0]*u[1], u[1]*u[1];
+ torque=Ginv*(v+F*uf);
- for(unsigned int i=0;i < fext.size();i++) fext[i].Zero();
-
- for(unsigned int i=0;i < joints_.size();i++)
- std::cout << "phi=" << phi(i) << " nu=" << nu(i)
- << " dnu=" << dnu(i) << std::endl;
-
- // Compute linearization.
- if(idsolver->CartToJnt(phi,nu,dnu,fext,torque) < 0) ROS_ERROR("KDL inverse dynamics solver failed.");
-
- for(unsigned int i=0;i < joints_.size(); i++)
- std::cout << "torque=" << torque(i) << std::endl;
-
// Apply torques
for(unsigned int i=0;i < joints_.size();i++) joints_[i].setCommand(torque(i));
}
--- /dev/null
+#include <twil_controllers/cart_linearizing_controller_rne.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <kdl/frames.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+
+namespace twil_controllers
+{
+ CartLinearizingController_RNE::CartLinearizingController_RNE(void):
+ phi(0),nu(0),dnu(0),torque(0),v(0),fext(0),wheelRadius(0)
+ {
+ }
+
+ CartLinearizingController_RNE::~CartLinearizingController_RNE(void)
+ {
+ sub_command_.shutdown();
+ }
+
+ bool CartLinearizingController_RNE::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->getJointHandle((std::string)name_value);
+ joints_.push_back(j);
+ v.push_back(0);
+ }
+ sub_command_ = node_.subscribe("command",1000,&CartLinearizingController_RNE::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;
+ }
+
+ KDL::Chain chain;
+ if (!tree.getChain("left_wheel","right_wheel",chain))
+ {
+ ROS_ERROR("Failed to get chain from KDL tree.");
+ return false;
+ }
+
+ // Get gravity from gazebo or set values if not simulating
+ KDL::Vector g;
+ node_.param("/gazebo/gravity_x",g[1],0.0);
+ node_.param("/gazebo/gravity_y",g[1],0.0);
+ node_.param("/gazebo/gravity_z",g[2],-9.8);
+
+ if((idsolver=new KDL::ChainIdSolver_RNE(chain,g)) == NULL)
+ {
+ ROS_ERROR("Failed to create ChainIDSolver_RNE.");
+ 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(chain.getNrOfJoints());
+ 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();
+
+ // set vectors to the right size
+ phi.resize(chain.getNrOfJoints());
+ nu.resize(chain.getNrOfJoints());
+ dnu.resize(chain.getNrOfJoints());
+ torque.resize(chain.getNrOfJoints());
+
+ fext.resize(chain.getNrOfSegments());
+
+ return true;
+ }
+
+ void CartLinearizingController_RNE::starting(const ros::Time& time)
+ {
+ last_time_=time;
+ for(unsigned int i=0; i < joints_.size();i++) v[i]=0.0;
+ }
+
+ void CartLinearizingController_RNE::update(const ros::Time& time)
+ {
+ ros::Duration dt=time-last_time_;
+ last_time_=time;
+
+ for(unsigned int i=0;i < joints_.size();i++)
+ {
+ phi(i)=joints_[i].getPosition();
+ nu(i)=joints_[i].getVelocity();
+ }
+
+
+ dnu(0)=v[0]/wheelRadius[0]-v[1]*wheelBase/2.0/wheelRadius[0]; // left wheel
+ dnu(1)=v[0]/wheelRadius[1]+v[1]*wheelBase/2.0/wheelRadius[1]; // right wheel
+
+
+ for(unsigned int i=0;i < fext.size();i++) fext[i].Zero();
+
+// for(unsigned int i=0;i < joints_.size();i++)
+// std::cout << "phi=" << phi(i) << " nu=" << nu(i)
+// << " dnu=" << dnu(i) << std::endl;
+
+ // Compute linearization.
+ if(idsolver->CartToJnt(phi,nu,dnu,fext,torque) < 0) ROS_ERROR("KDL inverse dynamics solver failed.");
+
+// for(unsigned int i=0;i < joints_.size(); i++)
+// std::cout << "torque=" << torque(i) << std::endl;
+
+ // Apply torques
+ for(unsigned int i=0;i < joints_.size();i++) joints_[i].setCommand(torque(i));
+ }
+
+ void CartLinearizingController_RNE::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command)
+ {
+ for(unsigned int i=0;i < command->data.size();i++) v[i]=command->data[i];
+ }
+}
+PLUGINLIB_DECLARE_CLASS(twil_controllers,CartLinearizingController_RNE,twil_controllers::CartLinearizingController_RNE,controller_interface::ControllerBase)
--- /dev/null
+#include <twil_controllers/nonsmooth_backstep_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <kdl/frames.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+
+#define sqr(x) (x*x)
+#define cub(x) (x*x*x)
+
+#define NUMERICAL_DETA
+
+namespace twil_controllers
+{
+ NonSmoothBackstepController::NonSmoothBackstepController(void):
+ wheelRadius(2)
+ {
+ }
+
+ NonSmoothBackstepController::~NonSmoothBackstepController(void)
+ {
+ sub_command_.shutdown();
+ }
+
+ bool NonSmoothBackstepController::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->getJointHandle((std::string)name_value);
+ joints_.push_back(j);
+ }
+ sub_command_=node_.subscribe("command",1000,&NonSmoothBackstepController::commandCB,this);
+ sub_parameters_=node_.subscribe("dynamic_parameters",1000,&NonSmoothBackstepController::parametersCB,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 NonSmoothBackstepController::starting(const ros::Time& time)
+ {
+ last_time_=time;
+ xi.setZero();
+ xiRef.setZero();
+ eta.setZero();
+
+ }
+
+ void NonSmoothBackstepController::update(const ros::Time& time)
+ {
+ ros::Duration dt=time-last_time_;
+ if(dt.toSec() < 0.01) return;
+ last_time_=time;
+
+ Eigen::Vector2d nu;
+ for(unsigned int i=0;i < joints_.size();i++)
+ {
+ nu[i]=joints_[i].getVelocity();
+ }
+
+ Eigen::Vector2d u;
+ u[0]=(nu[0]*wheelRadius[0]+nu[1]*wheelRadius[1])/2.0;
+ u[1]=(nu[1]*wheelRadius[1]-nu[0]*wheelRadius[0])/wheelBase;
+
+ // Estimate pose by odometry
+ Eigen::MatrixXd B(3,2);
+ B << cos(xi[2]+u[1]*dt.toSec()/2.0), 0.0,
+ sin(xi[2]+u[1]*dt.toSec()/2.0), 0.0,
+ 0.0, 1.0;
+
+ xi+=B*u*dt.toSec();
+
+ // Change of coordinates
+ Eigen::Matrix3d R;
+ R << cos(xiRef[2]), sin(xiRef[2]), 0.0,
+ -sin(xiRef[2]), cos(xiRef[2]), 0.0,
+ 0.0, 0.0, 1.0;
+ Eigen::Vector3d xBar=R*(xi-xiRef);
+
+ // Discontinuous transformation
+ double e=sqrt(sqr(xBar[0])+sqr(xBar[1]));
+ double psi=atan2(xBar[1],xBar[0]);
+ double alpha=xBar[2]-psi;
+
+ // Backstep controller
+ const double GAMMA1=10.0;
+ const double GAMMA2=1.0;
+// const double GAMMA3=1.0;
+ const double GAMMA4=10.0;
+ const double GAMMA5=1.0;
+ const double LAMBDA1=1.0;
+ const double LAMBDA2=0.1;
+ const double LAMBDA3=0.1;
+ const double LAMBDA4=10.0;
+ const double LAMBDA5=100.0;
+
+#ifdef NUMERICAL_DETA
+ Eigen::Vector2d deta=-eta;
+#endif
+ Eigen::Vector2d eta;
+
+ eta[0]=-GAMMA1*e*cos(alpha);
+
+ if(alpha > 1e-6) eta[1]=-GAMMA2*alpha-GAMMA1*sin(alpha)*cos(alpha)+GAMMA1*LAMBDA3*psi*sin(alpha)/LAMBDA2/alpha*cos(alpha);
+ else eta[1]=GAMMA1*LAMBDA3*psi/LAMBDA2;
+
+ Eigen::Vector2d eb=u-eta;
+
+ Eigen::Vector2d vBar;
+ if(e > 1e-6) vBar[0]=-GAMMA4*eb[0]-LAMBDA1/LAMBDA4*cos(alpha)
+ +LAMBDA2/LAMBDA4*alpha*sin(alpha)/e
+ -LAMBDA3/LAMBDA4*psi*sin(alpha)/e;
+ else vBar[0]=-GAMMA4*eb[0]-LAMBDA1/LAMBDA4*cos(alpha);
+ vBar[1]=-GAMMA5*eb[1]-LAMBDA2/LAMBDA5*alpha;
+
+#ifndef NUMERICAL_DETA
+ // Analitic derivates
+ Eigen::Vector2d deta;
+ deta[0]=sqr(GAMMA1)*e*cub(cos(alpha))-GAMMA1*GAMMA2*e*alpha*sin(alpha)
+ +sqr(GAMMA1)*LAMBDA3/LAMBDA2*e*cos(alpha)*sqr(sin(alpha))/alpha*psi;
+ deta[1]=sqr(GAMMA2)*alpha-2.0*GAMMA1*GAMMA2*LAMBDA3/LAMBDA2*psi*sin(alpha)/alpha*cos(alpha)
+ +GAMMA1*GAMMA2*alpha*sqr(cos(alpha))-sqr(GAMMA1)*LAMBDA3/LAMBDA2*cub(cos(alpha))*sin(alpha)/alpha*psi
+ -GAMMA1*GAMMA2*alpha*sqr(sin(alpha))+sqr(GAMMA1)*LAMBDA3/LAMBDA2*cos(alpha)*cub(sin(alpha))/alpha*psi
+ -sqr(GAMMA1)*LAMBDA3/LAMBDA2*sqr(cos(alpha))*sqr(sin(alpha))/alpha-GAMMA1*GAMMA2*LAMBDA3/LAMBDA2*psi
+ +sqr(GAMMA1*LAMBDA3/LAMBDA2)*cub(cos(alpha))*sin(alpha)/sqr(alpha)*sqr(psi)+sqr(GAMMA1*LAMBDA3/LAMBDA2)*cos(alpha)*cub(sin(alpha))/sqr(alpha)*sqr(psi)
+ +sqr(GAMMA1*LAMBDA3/LAMBDA2*cos(alpha)*sin(alpha)*psi)/cub(alpha);
+#else
+ deta+=eta;
+ deta/=dt.toSec();
+#endif
+
+ Eigen::Vector2d v=vBar+deta;
+
+
+ // Linearization.
+ Eigen::Vector2d uf;
+ uf << u[0]*u[1], sqr(u[1]);
+
+ Eigen::Vector2d torque=Ginv*(v+F*uf);
+
+ // Apply torques
+ for(unsigned int i=0;i < joints_.size();i++)
+ {
+ if(torque[i] > 100.0) torque[i]=100.0;
+ if(torque[i] < -100.0) torque[i]=-100.0;
+ joints_[i].setCommand(torque[i]);
+ }
+ }
+
+ void NonSmoothBackstepController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command)
+ {
+ for(unsigned int i=0;i < command->data.size() && i < 2;i++) xiRef[i]=command->data[i];
+ }
+
+ void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command)
+ {
+ // data[0]=K5, data[1]=K6, data[2]=K7, data[3]=K8
+ // data[4]=Pk5, data[5]=Pk6, data[6]=Pk7, data[7]=Pk8
+ // F= [0 K5
+ // K6 0]
+ // Ginv=[0.5K7 0.5K8
+ // 0.5K7 -0.5K8
+
+ if(command->data[4] < 1e-3) F(0,1)=command->data[0];
+ if(command->data[5] < 1e-3) F(1,0)=command->data[1];
+ if(command->data[6] < 1e-3)
+ {
+ Ginv(0,0)=0.5*command->data[2];
+ Ginv(1,0)=0.5*command->data[2];
+ }
+ if(command->data[7] < 1e-3)
+ {
+ Ginv(0,1)=0.5*command->data[3];
+ Ginv(1,1)=-0.5*command->data[3];
+ }
+ }
+
+}
+
+PLUGINLIB_DECLARE_CLASS(twil_controllers,NonSmoothBackstepController,twil_controllers::NonSmoothBackstepController,controller_interface::ControllerBase)
EffortJointInterface type of hardware interface.
</description>
</class>
+
+ <class name="twil_controllers/CartLinearizingController_RNE" 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. Recursive
+ Newton-Euler from KDL is used to compute torque. Not working
+ properly.
+ </description>
+ </class>
+
+ <class name="twil_controllers/NonSmoothBackstepController" type="twil_controllers::NonSmoothBackstepController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The NonSmoothBackstepController implements a non-smooth controller
+ based on a discontinuous transformation and backstepping. A feedback
+ linearization of the Twil dynamic model is also performed. The
+ inputs to the controller are the reference Cartesian pose. It expects a
+ EffortJointInterface type of hardware interface.
+ </description>
+ </class>
</library>
projects / twil.git / commitdiff