From: Walter Fetter Lages Date: Wed, 21 Mar 2018 21:38:11 +0000 (-0300) Subject: remove twil_trajectories X-Git-Tag: petryTCC~18 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=3f612386cd13dc6248f9a91e957941d3bd91eec3;p=twil.git remove twil_trajectories twil_description: fix bug in display.launch fix xacro addresses remove namespace twil_ident: remove namespace add license and copyright add Eigen twil_controllers: remove cart_linearizing_rne remove nonsmooth_backstep_controller --- diff --git a/twil/package.xml b/twil/package.xml index 5d03e91..2d4a31b 100644 --- a/twil/package.xml +++ b/twil/package.xml @@ -45,11 +45,7 @@ - twil_controllers twil_ident - diff --git a/twil_controllers/CMakeLists.txt b/twil_controllers/CMakeLists.txt index 5fd9edb..ca45ac9 100644 --- a/twil_controllers/CMakeLists.txt +++ b/twil_controllers/CMakeLists.txt @@ -111,8 +111,6 @@ include_directories(include ## Declare a cpp library add_library(twil_controllers src/cart_linearizing_controller.cpp - src/cart_linearizing_controller_rne.cpp - src/nonsmooth_backstep_controller.cpp ) ## Declare a cpp executable diff --git a/twil_controllers/config/nonsmooth_backstep_control.yaml b/twil_controllers/config/nonsmooth_backstep_control.yaml deleted file mode 100644 index 6ff08e6..0000000 --- a/twil_controllers/config/nonsmooth_backstep_control.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# Watch-out: The indentation here is relevant to the semantic! - -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 - lambda: [10.0, 0.1, 0.1, 50.0, 100.0] - gamma: [10.0, 1.0, 10.0, 10.0] diff --git a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h b/twil_controllers/include/twil_controllers/cart_linearizing_controller.h index 2ba297d..1ae6bc3 100644 --- a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h +++ b/twil_controllers/include/twil_controllers/cart_linearizing_controller.h @@ -1,3 +1,24 @@ +/****************************************************************************** + ROS twil_controllers Package + Cartesian Linearizing Controller + Copyright (C) 2014..2017 Walter Fetter Lages + + 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 + . + +*******************************************************************************/ + #ifndef TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H #define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H diff --git a/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h b/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h deleted file mode 100644 index 20df5d3..0000000 --- a/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_RNE_H -#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_RNE_H - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - - -namespace twil_controllers -{ - class CartLinearizingController_RNE: public controller_interface::Controller - { - 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,const ros::Duration& duration); - - private: - ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; - std::vector 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 v; - - KDL::Wrenches fext; - - std::vector wheelRadius; - double wheelBase; - }; -} -#endif diff --git a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h b/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h deleted file mode 100644 index f37692e..0000000 --- a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H -#define TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include - -namespace twil_controllers -{ - class NonSmoothBackstepController: public controller_interface:: - Controller - { - 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,const ros::Duration& duration); - - private: - ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; - std::vector joints_; - - boost::scoped_ptr - > status_publisher_ ; - - boost::shared_ptr > odom_publisher_; - boost::shared_ptr > tf_odom_publisher_; - - ros::Subscriber sub_command_; - ros::Subscriber sub_parameters_; - - Eigen::Matrix2d Ginv; - Eigen::Matrix2d F; - - std::vector wheelRadius; - double wheelBase; - - Eigen::Vector3d x; - Eigen::Vector3d xRef; - - Eigen::Vector2d eta; - - ros::Time lastSamplingTime; - - Eigen::Vector2d phi; - - std::vector lambda; - std::vector gamma; - - void commandCB(const geometry_msgs::Pose2D::ConstPtr &command); - void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command); - }; -} -#endif diff --git a/twil_controllers/launch/adaptive_nonsmooth_backstep.launch b/twil_controllers/launch/adaptive_nonsmooth_backstep.launch deleted file mode 100644 index ac31acb..0000000 --- a/twil_controllers/launch/adaptive_nonsmooth_backstep.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - diff --git a/twil_controllers/launch/gazebo_nonsmooth_backstep.launch b/twil_controllers/launch/gazebo_nonsmooth_backstep.launch deleted file mode 100644 index 16002aa..0000000 --- a/twil_controllers/launch/gazebo_nonsmooth_backstep.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch b/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch deleted file mode 100644 index 6375e93..0000000 --- a/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/twil_controllers/launch/nonsmooth_backstep.launch b/twil_controllers/launch/nonsmooth_backstep.launch deleted file mode 100644 index 695b6e3..0000000 --- a/twil_controllers/launch/nonsmooth_backstep.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - diff --git a/twil_controllers/msg/NonSmoothBackstepControllerStatus.msg b/twil_controllers/msg/NonSmoothBackstepControllerStatus.msg deleted file mode 100644 index 3012416..0000000 --- a/twil_controllers/msg/NonSmoothBackstepControllerStatus.msg +++ /dev/null @@ -1,16 +0,0 @@ -Header header -geometry_msgs/Pose2D set_point -geometry_msgs/Pose2D process_value -geometry_msgs/Pose2D process_value_dot -geometry_msgs/Pose2D error -float64 time_step -float64[2] command -float64[5] lambda -float64[4] gamma -PosePolar polar_error -float64[2] backstep_set_point -float64[2] backstep_set_point_dot -float64[2] backstep_process_value -float64[2] backstep_error -float64[2] backstep_command -float64[2] linear_dynamics_command diff --git a/twil_controllers/msg/PosePolar.msg b/twil_controllers/msg/PosePolar.msg deleted file mode 100644 index 14a00fa..0000000 --- a/twil_controllers/msg/PosePolar.msg +++ /dev/null @@ -1,3 +0,0 @@ -float64 range -float64 angle -float64 orientation diff --git a/twil_controllers/scripts/pose_step.py b/twil_controllers/scripts/pose_step.py deleted file mode 100755 index 1bb1007..0000000 --- a/twil_controllers/scripts/pose_step.py +++ /dev/null @@ -1,32 +0,0 @@ -#!/usr/bin/python - -import sys -import time - -import rospy -from geometry_msgs.msg import Pose2D - -def step(): - if len(sys.argv) < 4: - print 'pose_step.py x y theta' - exit() - pose=Pose2D() - pose.x=float(sys.argv[1]) - pose.y=float(sys.argv[2]) - pose.theta=float(sys.argv[3]) - - pub=rospy.Publisher('/twil/nonsmooth_backstep_controller/command', Pose2D, queue_size=1) - - rospy.init_node('pose_step',anonymous=True) - - pub.publish(pose) - time.sleep(1) - - pub.publish(pose) - time.sleep(1) - -if __name__ == '__main__': - try: - step() - except rospy.ROSInterruptException: - pass diff --git a/twil_controllers/scripts/pose_step.sh b/twil_controllers/scripts/pose_step.sh deleted file mode 100755 index 4d8e303..0000000 --- a/twil_controllers/scripts/pose_step.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -rostopic pub -1 /twil/nonsmooth_backstep_controller/command geometry_msgs/Pose2D "{x: $1, y: $2, theta: $3}" diff --git a/twil_controllers/src/cart_linearizing_controller.cpp b/twil_controllers/src/cart_linearizing_controller.cpp index b411e7f..f04a5cd 100644 --- a/twil_controllers/src/cart_linearizing_controller.cpp +++ b/twil_controllers/src/cart_linearizing_controller.cpp @@ -1,3 +1,24 @@ +/****************************************************************************** + ROS twil_controllers Package + Cartesian Linearizing Controller + Copyright (C) 2014..2017 Walter Fetter Lages + + 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 + . + +*******************************************************************************/ + #include #include diff --git a/twil_controllers/src/cart_linearizing_controller_rne.cpp b/twil_controllers/src/cart_linearizing_controller_rne.cpp deleted file mode 100644 index 31fc7e7..0000000 --- a/twil_controllers/src/cart_linearizing_controller_rne.cpp +++ /dev/null @@ -1,159 +0,0 @@ -#include -#include - -#include -#include -#include - -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->getHandle((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) - { - for(unsigned int i=0; i < joints_.size();i++) v[i]=0.0; - } - - void CartLinearizingController_RNE::update(const ros::Time& time, - const ros::Duration& duration) - { - 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_EXPORT_CLASS(twil_controllers::CartLinearizingController_RNE,controller_interface::ControllerBase) diff --git a/twil_controllers/src/nonsmooth_backstep_controller.cpp b/twil_controllers/src/nonsmooth_backstep_controller.cpp deleted file mode 100644 index b2acae8..0000000 --- a/twil_controllers/src/nonsmooth_backstep_controller.cpp +++ /dev/null @@ -1,394 +0,0 @@ -#include - -#include -#include -#include - -#include - -#define sqr(x) (x*x) -#define cub(x) (x*x*x) - -namespace twil_controllers -{ - NonSmoothBackstepController::NonSmoothBackstepController(void): - wheelRadius(2),lambda(5),gamma(4) - { - } - - NonSmoothBackstepController::~NonSmoothBackstepController(void) - { - sub_command_.shutdown(); - sub_parameters_.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->getHandle((std::string)name_value); - joints_.push_back(j); - } - - status_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"status",1)); - status_publisher_->msg_.header.frame_id="odom"; - - odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"odom",100)); - odom_publisher_->msg_.header.frame_id="odom"; - odom_publisher_->msg_.child_frame_id="twil"; - odom_publisher_->msg_.pose.pose.position.z=0; - odom_publisher_->msg_.pose.pose.orientation.x=0; - odom_publisher_->msg_.pose.pose.orientation.y=0; - - // Fake covariance - double pose_cov[]={1e-6, 1e-6, 1e-16,1e-16,1e-16,1e-9}; - odom_publisher_->msg_.pose.covariance=boost::assign::list_of - (pose_cov[0]) (0) (0) (0) (0) (0) - (0) (pose_cov[1]) (0) (0) (0) (0) - (0) (0) (pose_cov[2]) (0) (0) (0) - (0) (0) (0) (pose_cov[3]) (0) (0) - (0) (0) (0) (0) (pose_cov[4]) (0) - (0) (0) (0) (0) (0) (pose_cov[5]); - - odom_publisher_->msg_.twist.twist.linear.z=0; - odom_publisher_->msg_.twist.twist.angular.x=0; - odom_publisher_->msg_.twist.twist.angular.y=0; - - //Fake covariance - double twist_cov[]={1e-6,1e-6,1e-16,1e-16,1e-16,1e-9}; - odom_publisher_->msg_.twist.covariance=boost::assign::list_of - (twist_cov[0]) (0) (0) (0) (0) (0) - (0) (twist_cov[1]) (0) (0) (0) (0) - (0) (0) (twist_cov[2]) (0) (0) (0) - (0) (0) (0) (twist_cov[3]) (0) (0) - (0) (0) (0) (0) (twist_cov[4]) (0) - (0) (0) (0) (0) (0) (twist_cov[5]); - - tf_odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"/tf",100)); - tf_odom_publisher_->msg_.transforms.resize(1); - tf_odom_publisher_->msg_.transforms[0].transform.translation.z=0.0; - tf_odom_publisher_->msg_.transforms[0].transform.rotation.x=0.0; - tf_odom_publisher_->msg_.transforms[0].transform.rotation.y=0.0; - tf_odom_publisher_->msg_.transforms[0].child_frame_id="twil_origin"; - tf_odom_publisher_->msg_.transforms[0].header.frame_id="odom"; - - 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(); - - gamma[0]=10.0; - gamma[1]=1.0; - gamma[2]=10.0; - gamma[3]=10.0; - node_.param("gamma",gamma,gamma); - - lambda[0]=10.0; - lambda[1]=0.1; - lambda[2]=0.1; - lambda[3]=50.0; - lambda[4]=100.0; - node_.param("lambda",lambda,lambda); - - const double K5=0.08444758509282763; - const double K6=3.770688129256381; - const double K7=2.6468901285322475; - const double K8=-16.084061415321404; - - 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) - { - x.setZero(); - xRef.setZero(); - eta.setZero(); - lastSamplingTime=time; - for(unsigned int i=0;i < joints_.size();i++) - { - phi[i]=joints_[i].getPosition(); - } - } - - void NonSmoothBackstepController::update(const ros::Time& time, - const ros::Duration& duration) - { - double dt=time.toSec()-lastSamplingTime.toSec(); - - if(fabs(dt-0.01) > 0.001/2) return; - lastSamplingTime=time; - - // Incremental encoders sense angular displacement and - // not velocity - // phi[0] is the left wheel angular displacement - // phi[1] is the right wheel angular displacement - Eigen::Vector2d deltaPhi=-phi; - for(unsigned int i=0;i < joints_.size();i++) - { - phi[i]=joints_[i].getPosition(); - } - deltaPhi+=phi; - - // Estimate pose by odometry - Eigen::Vector2d deltaU; - deltaU << (deltaPhi[0]*wheelRadius[0]+deltaPhi[1]*wheelRadius[1])/2.0, - (deltaPhi[1]*wheelRadius[1]-deltaPhi[0]*wheelRadius[0])/wheelBase; - - double deltaS=(fabs(deltaU[1]) > 1e-10)? - deltaU[0]*sin(deltaU[1]/2)/(deltaU[1]/2):deltaU[0]; - - Eigen::Vector3d deltaX; - deltaX << deltaS*cos(x[2]+deltaU[1]/2.0), - deltaS*sin(x[2]+deltaU[1]/2.0), - deltaU[1]; - - x+=deltaX; - - Eigen::Vector2d u=deltaU/dt; - - // Change of coordinates - Eigen::Matrix3d R; - R << cos(xRef[2]), sin(xRef[2]), 0.0, - -sin(xRef[2]), cos(xRef[2]), 0.0, - 0.0, 0.0, 1.0; - Eigen::Vector3d xBar=R*(x-xRef); - - // Discontinuous transformation - double e=sqrt(sqr(xBar[0])+sqr(xBar[1])); - double psi=atan2(xBar[1],xBar[0]); - double alpha=xBar[2]-psi; - - // Backstepping - eta[0]=-gamma[0]*e*cos(alpha); - - if(fabs(alpha ) > 1e-10) - eta[1]=-gamma[1]*alpha - -gamma[0]*sin(alpha)*cos(alpha)+gamma[0]*lambda[2]*psi*sin(alpha)/ - lambda[1]/alpha*cos(alpha); - else eta[1]=gamma[0]*lambda[2]*psi/lambda[1]; - - Eigen::Vector2d eb=u-eta; - - Eigen::Vector2d vBar; - if(fabs(e) > 1e-10) vBar[0]=-gamma[2]*eb[0]-lambda[0]/lambda[3]*cos(alpha) - +lambda[1]/lambda[3]*alpha*sin(alpha)/e - -lambda[2]/lambda[3]*psi*sin(alpha)/e; - else vBar[0]=-gamma[2]*eb[0]-lambda[0]/lambda[3]*cos(alpha); - vBar[1]=-gamma[3]*eb[1]-lambda[1]/lambda[4]*alpha; - - Eigen::Vector2d deta; - - deta[0]=e*cub(cos(alpha))*sqr(gamma[0])-e*gamma[0]*sin(alpha)*(alpha*gamma[1] - -(gamma[0]*lambda[2]*cos(alpha)*sin(alpha)*psi)/(alpha*lambda[1])); - - deta[1]=gamma[1]*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) - +gamma[0]*sqr(cos(alpha))*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) - -gamma[0]*sqr(sin(alpha))*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) - -(lambda[2]*sqr(cos(alpha))*sqr(sin(alpha))*sqr(gamma[0]))/(alpha*lambda[1]) - -(gamma[0]*lambda[2]*psi*sqr(cos(alpha))*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) - /(alpha*lambda[1]) - +(gamma[0]*lambda[2]*psi*sqr(sin(alpha))*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) - /(alpha*lambda[1]) - +(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha)*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) - /(sqr(alpha)*lambda[1]); - - Eigen::Vector2d v=vBar+deta; - - // Linearization - Eigen::Vector2d 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++) - { - joints_[i].setCommand(torque[i]); - } - - if(status_publisher_ && status_publisher_->trylock()) - { - status_publisher_->msg_.header.stamp=time; - - status_publisher_->msg_.set_point.x=xRef[0]; - status_publisher_->msg_.set_point.y=xRef[1]; - status_publisher_->msg_.set_point.theta=xRef[2]; - - status_publisher_->msg_.process_value.x=x[0]; - status_publisher_->msg_.process_value.y=x[1]; - status_publisher_->msg_.process_value.theta=x[2]; - - status_publisher_->msg_.process_value_dot.x=deltaX[0]/dt; - status_publisher_->msg_.process_value_dot.y=deltaX[1]/dt; - status_publisher_->msg_.process_value_dot.theta=deltaX[2]/dt; - - status_publisher_->msg_.error.x=xRef[0]-x[0]; - status_publisher_->msg_.error.y=xRef[1]-x[1]; - status_publisher_->msg_.error.theta=xRef[2]-x[2]; - - status_publisher_->msg_.time_step=dt; - - for(int i=0;i < torque.size();i++) - status_publisher_->msg_.command[i]=torque[i]; - - for(int i=0;i < lambda.size();i++) - status_publisher_->msg_.lambda[i]=lambda[i]; - - for(int i=0;i < gamma.size();i++) - status_publisher_->msg_.gamma[i]=gamma[i]; - - status_publisher_->msg_.polar_error.range=e; - status_publisher_->msg_.polar_error.angle=psi; - status_publisher_->msg_.polar_error.orientation=alpha; - - for(int i=0;i < eta.size();i++) - status_publisher_->msg_.backstep_set_point[i]=eta[i]; - - for(int i=0;i < deta.size();i++) - status_publisher_->msg_.backstep_set_point_dot[i]=deta[i]; - - for(int i=0;i < u.size();i++) - status_publisher_->msg_.backstep_process_value[i]=u[i]; - - for(int i=0;i < eb.size();i++) - status_publisher_->msg_.backstep_error[i]=eb[i]; - - for(int i=0;i < vBar.size();i++) - status_publisher_->msg_.backstep_command[i]=vBar[i]; - - for(int i=0;i < v.size();i++) - status_publisher_->msg_.linear_dynamics_command[i]=v[i]; - - status_publisher_->unlockAndPublish(); - } - - if(odom_publisher_ && odom_publisher_->trylock()) - { - odom_publisher_->msg_.header.stamp=time; - - odom_publisher_->msg_.pose.pose.position.x=x[0]; - odom_publisher_->msg_.pose.pose.position.y=x[1]; - odom_publisher_->msg_.pose.pose.orientation.z=sin(x[2]/2); - odom_publisher_->msg_.pose.pose.orientation.w=cos(x[2]/2); - - odom_publisher_->msg_.twist.twist.linear.x=deltaX[0]/dt; - odom_publisher_->msg_.twist.twist.linear.y=deltaX[1]/dt; - odom_publisher_->msg_.twist.twist.angular.z=deltaX[2]/dt; - - odom_publisher_->unlockAndPublish(); - } - - if(tf_odom_publisher_ && tf_odom_publisher_->trylock()) - { - geometry_msgs::TransformStamped &odom_frame= - tf_odom_publisher_->msg_.transforms[0]; - odom_frame.header.stamp=time; - odom_frame.transform.translation.x=x[0]; - odom_frame.transform.translation.y=x[1]; - odom_frame.transform.rotation.z=sin(x[2]/2); - odom_frame.transform.rotation.w=cos(x[2]/2); - - tf_odom_publisher_->unlockAndPublish(); - } - } - - void NonSmoothBackstepController::commandCB(const geometry_msgs::Pose2D::ConstPtr &command) - { - xRef[0]=command->x; - xRef[1]=command->y; - xRef[2]=command->theta; - } - - void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr ¶m) - { - // 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.5/K7 0.5/K8 - // 0.5/K7 -0.5/K8] - - if(param->data[4] < 1e-3) F(0,1)=param->data[0]; - if(param->data[5] < 1e-3) F(1,0)=param->data[1]; - if(param->data[6] < 1e-3) - { - Ginv(0,0)=0.5/param->data[2]; - Ginv(1,0)=0.5/param->data[2]; - } - if(param->data[7] < 1e-3) - { - Ginv(0,1)=0.5/param->data[3]; - Ginv(1,1)=-0.5/param->data[3]; - } - } - -} - -PLUGINLIB_EXPORT_CLASS(twil_controllers::NonSmoothBackstepController,controller_interface::ControllerBase) diff --git a/twil_controllers/twil_controllers_plugins.xml b/twil_controllers/twil_controllers_plugins.xml index 15684e6..a2346e0 100644 --- a/twil_controllers/twil_controllers_plugins.xml +++ b/twil_controllers/twil_controllers_plugins.xml @@ -8,24 +8,4 @@ - - - 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. - - - - - - 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. - - - diff --git a/twil_description/launch/display.launch b/twil_description/launch/display.launch index 86ab474..17abacf 100644 --- a/twil_description/launch/display.launch +++ b/twil_description/launch/display.launch @@ -6,7 +6,7 @@ - + diff --git a/twil_description/rviz/display.rviz b/twil_description/rviz/display.rviz index d25d868..a7b228d 100644 --- a/twil_description/rviz/display.rviz +++ b/twil_description/rviz/display.rviz @@ -167,7 +167,7 @@ Visualization Manager: Length: 1 Name: Odometry Position Tolerance: 0.3 - Topic: /twil/nonsmooth_backstep_controller/odom + Topic: /nonsmooth_backstep_controller/odom Value: true - Alpha: 1 Axes Length: 1 @@ -181,7 +181,7 @@ Visualization Manager: Shaft Length: 1 Shaft Radius: 0.05 Shape: Arrow - Topic: /twil/command_stamped + Topic: /command_stamped Value: true Enabled: true Global Options: diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro index fa32a0c..4d1321c 100644 --- a/twil_description/xacro/twil.urdf.xacro +++ b/twil_description/xacro/twil.urdf.xacro @@ -1,5 +1,5 @@ - + @@ -97,7 +97,7 @@ - /twil + diff --git a/twil_description/xacro/twil_wam.urdf.xacro b/twil_description/xacro/twil_wam.urdf.xacro index cbe14dd..9074f66 100644 --- a/twil_description/xacro/twil_wam.urdf.xacro +++ b/twil_description/xacro/twil_wam.urdf.xacro @@ -1,5 +1,5 @@ - + diff --git a/twil_ident/CMakeLists.txt b/twil_ident/CMakeLists.txt index 55300bb..0bee681 100644 --- a/twil_ident/CMakeLists.txt +++ b/twil_ident/CMakeLists.txt @@ -91,9 +91,10 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations -# include_directories(include) -# TODO: Check names of system library include directories (eigen) include_directories( + include + ${catkin_INCLUDE_DIRS} +# TODO: Check names of system library include directories (eigen) ${Eigen_INCLUDE_DIRS} ) diff --git a/twil_ident/launch/gazebo.launch b/twil_ident/launch/gazebo.launch index df6206f..a18f7f6 100644 --- a/twil_ident/launch/gazebo.launch +++ b/twil_ident/launch/gazebo.launch @@ -3,11 +3,11 @@ - - + + - - + + diff --git a/twil_ident/launch/ident.launch b/twil_ident/launch/ident.launch index 3e6e69a..e939edf 100644 --- a/twil_ident/launch/ident.launch +++ b/twil_ident/launch/ident.launch @@ -1,5 +1,5 @@ - + diff --git a/twil_ident/src/ident.cpp b/twil_ident/src/ident.cpp index c1ffaf5..88e6fb0 100644 --- a/twil_ident/src/ident.cpp +++ b/twil_ident/src/ident.cpp @@ -1,3 +1,24 @@ +/****************************************************************************** + ROS twil_indet Package + Twil Dynamics Model + Copyright (C) 2014..2017 Walter Fetter Lages + + 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 + . + +*******************************************************************************/ + #include #include diff --git a/twil_trajectories/CMakeLists.txt b/twil_trajectories/CMakeLists.txt deleted file mode 100644 index a38b1ef..0000000 --- a/twil_trajectories/CMakeLists.txt +++ /dev/null @@ -1,177 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(twil_trajectories) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp) - -find_package(cmake_modules REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) -find_package(Eigen REQUIRED) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## 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 other_catkin_pkg - DEPENDS eigen -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -# TODO: Check names of system library include directories (eigen) -include_directories( - include ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} -) - -## Declare a cpp library -add_library(${PROJECT_NAME} - src/circle_path.cpp - src/eight_path.cpp -) - -## Declare a cpp executable -add_executable(eight_trajectory src/eight_trajectory.cpp) -add_executable(pose2d_stamp src/pose2d_stamp.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(twil_trajectories_node twil_trajectories_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${eigen_LIBRARIES} -) - -target_link_libraries(eight_trajectory - ${catkin_LIBRARIES} - ${eigen_LIBRARIES} - ${PROJECT_NAME} -) - -target_link_libraries(pose2d_stamp - ${catkin_LIBRARIES} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## 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} -) - -## Mark cpp header files for installation -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 -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_trajectories.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/twil_trajectories/include/twil_trajectories/circle_path.h b/twil_trajectories/include/twil_trajectories/circle_path.h deleted file mode 100644 index 95c2464..0000000 --- a/twil_trajectories/include/twil_trajectories/circle_path.h +++ /dev/null @@ -1,77 +0,0 @@ -/****************************************************************************** - ROS twil_trajectories Package - Circle Path Class - Copyright (C) 2015..2017 Walter Fetter Lages - - 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 - . - -*******************************************************************************/ - -#ifndef CIRCLE_PATH_H -#define CIRCLE_PATH_H - -#include - -/** Circle path -* @author Walter Fetter Lages -*/ -class CirclePath -{ - Eigen::Vector2d pc_; - double phi0_; - double r_; - double w_; - - public: - - /** Build a circle path. - * @param pc circle center point. - * @param phi0 initial phase. - * @param r circle radius. - * @param w angular velocity. - */ - CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w); - - /** Build a circle path. - * @param p0 starting pose. - * @param r circle radius. - * @param w angular velocity. - */ - CirclePath(const Eigen::Vector3d &p0,double r,double w); - - /** Build a circle path. - * @param pc circle center point. - * @param p0 starting point. - * @param w angular velocity. - */ - CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w); - - /** Destroy a circle path. - */ - ~CirclePath(void) { }; - - /** Get path point. - * @param t path time. - * @return path point. - */ - Eigen::Vector3d point(double t) const; - - /** Get path steering controls. - * @param t path time. - * @return steering controls. - */ - Eigen::Vector2d steering(double t) const; -}; -#endif diff --git a/twil_trajectories/include/twil_trajectories/eight_path.h b/twil_trajectories/include/twil_trajectories/eight_path.h deleted file mode 100644 index d1b02fe..0000000 --- a/twil_trajectories/include/twil_trajectories/eight_path.h +++ /dev/null @@ -1,61 +0,0 @@ -/****************************************************************************** - ROS twil_trajectories Package - Eigth Path Class - Copyright (C) 2015..2017 Walter Fetter Lages - - 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 - . - -*******************************************************************************/ - -#ifndef EIGHT_PATH_H -#define EIGHT_PATH_H - -#include - -/** 8 path -* @author Walter Fetter Lages -*/ -class EightPath -{ - CirclePath c1_; - CirclePath c2_; - double period_; - - public: - - /** Build an 8 path. - * @param pc center point. - * @param r radius. - * @param w angular velocity. - */ - EightPath(const Eigen::Vector2d &pc,double r,double w); - - /** Destroy an 8 path. - */ - ~EightPath(void) { }; - - /** Get path point. - * @param t path time. - * @return path point. - */ - Eigen::Vector3d point(double t) const; - - /** Get path steering controls. - * @param t path time. - * @return steering controls. - */ - Eigen::Vector2d steering(double t) const; -}; -#endif diff --git a/twil_trajectories/package.xml b/twil_trajectories/package.xml deleted file mode 100644 index 8545852..0000000 --- a/twil_trajectories/package.xml +++ /dev/null @@ -1,58 +0,0 @@ - - - twil_trajectories - 2.0.0 - The twil_trajectories package - - - - - Walter Fetter Lages - - - - - - GPLv3 - - - - - - - - - - - - - Walter Fetter Lages - - - - - - - - - - - - - - catkin - eigen - geometry_msgs - - eigen - geometry_msgs - - - - - - - - - - diff --git a/twil_trajectories/src/circle_path.cpp b/twil_trajectories/src/circle_path.cpp deleted file mode 100644 index aa71f83..0000000 --- a/twil_trajectories/src/circle_path.cpp +++ /dev/null @@ -1,72 +0,0 @@ -/****************************************************************************** - ROS twil_trajectories Package - Circle Path Class - Copyright (C) 2015..2017 Walter Fetter Lages - - 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 - . - -*******************************************************************************/ - -#include - -#define sqr(x) (x*x) -#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x))) - -CirclePath::CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w) -{ - pc_=pc; - phi0_=phi0; - r_=r; - w_=w; -} - -CirclePath::CirclePath(const Eigen::Vector3d &p0,double r,double w) -{ - phi0_=p0[2]-sgn(w)*M_PI_2; - pc_[0]=p0[0]-r*cos(phi0_); - pc_[1]=p0[1]-r*sin(phi0_); - r_=r; - w_=w; -} - -CirclePath::CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w) -{ - pc_=pc; - w_=w; - phi0_=atan2(p0[1]-pc[1],p0[0]-pc[0]); - r_=sqrt(sqr(p0[1]-pc[1])+sqr(p0[0]-pc[0])); -} - -Eigen::Vector3d CirclePath::point(double t) const -{ - double wt=w_*t; - Eigen::Vector3d p; - - p[0]=pc_[0]+r_*cos(wt+phi0_); - p[1]=pc_[1]+r_*sin(wt+phi0_); - p[2]=wt+phi0_+sgn(w_)*M_PI_2; - - return p; -} - -Eigen::Vector2d CirclePath::steering(double t) const -{ - Eigen::Vector2d e(2); - - e[0]=w_*r_; - e[1]=w_; - - return e; -} diff --git a/twil_trajectories/src/eight_path.cpp b/twil_trajectories/src/eight_path.cpp deleted file mode 100644 index 0645aa7..0000000 --- a/twil_trajectories/src/eight_path.cpp +++ /dev/null @@ -1,47 +0,0 @@ -/****************************************************************************** - ROS twil_trajectories Package - Eigth Path Class - Copyright (C) 2015..2017 Walter Fetter Lages - - 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 - . - -*******************************************************************************/ - -#include - -#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x))) - -EightPath::EightPath(const Eigen::Vector2d &pc,double r,double w): -c1_(Eigen::Vector2d(pc[0],pc[1]+r),-M_PI_2*sgn(w),r,w), -c2_(Eigen::Vector2d(pc[0],pc[1]-r),2.5*M_PI*sgn(w),r,-w) -{ - period_=4*M_PI/w; -}; - -Eigen::Vector3d EightPath::point(double t) const -{ - double tc=fmod(t,period_/2.0); - - if(int(t/(period_/2.0)) % 2 == 0) return c1_.point(tc); - else return c2_.point(tc); -} - -Eigen::Vector2d EightPath::steering(double t) const -{ - double tc=fmod(t,period_/2.0); - - if(int(t/(period_/2.0)) % 2 == 0) return c1_.steering(tc); - else return c2_.steering(tc); -} diff --git a/twil_trajectories/src/eight_trajectory.cpp b/twil_trajectories/src/eight_trajectory.cpp deleted file mode 100644 index f7848bf..0000000 --- a/twil_trajectories/src/eight_trajectory.cpp +++ /dev/null @@ -1,94 +0,0 @@ -/****************************************************************************** - ROS twil_trajectories Package - Eigth Trajectory Node - Copyright (C) 2015..2017 Walter Fetter Lages - - 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 - . - -*******************************************************************************/ - -#include -#include - -#include - -class EightTrajectory -{ - public: - EightTrajectory(ros::NodeHandle node); - ~EightTrajectory(void); - void setCommand(ros::Duration t); - - private: - ros::NodeHandle node_; - const EightPath *path; - - ros::Publisher commandPublisher; -}; - - -EightTrajectory::EightTrajectory(ros::NodeHandle node) -{ - node_=node; - Eigen::Vector2d pc; - double w; - double r; - - ros::param::get("~x",pc[0]); - ros::param::get("~y",pc[1]); - ros::param::get("~radius",r); - ros::param::get("~ang_vel",w); - - path=new EightPath(pc,r,w); - commandPublisher=node_.advertise("command",1000); -} - -EightTrajectory::~EightTrajectory(void) -{ - commandPublisher.shutdown(); - delete path; -} - -void EightTrajectory::setCommand(ros::Duration t) -{ - Eigen::Vector3d p=path->point(t.toSec()); - - geometry_msgs::Pose2D command; - command.x=p[0]; - command.y=p[1]; - command.theta=p[2]; - commandPublisher.publish(command); -} - -int main(int argc,char* argv[]) -{ - - ros::init(argc,argv,"eight_trajectory"); - ros::NodeHandle node; - - EightTrajectory eightTrajectory(node); - - ros::Rate loop(100); - - ros::Time t0=ros::Time::now(); - while(ros::ok()) - { - eightTrajectory.setCommand(ros::Time::now()-t0); - - ros::spinOnce(); - loop.sleep(); - } - return 0; -} diff --git a/twil_trajectories/src/pose2d_stamp.cpp b/twil_trajectories/src/pose2d_stamp.cpp deleted file mode 100644 index 61a92a8..0000000 --- a/twil_trajectories/src/pose2d_stamp.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/****************************************************************************** - ROS twil_trajectories Package - Pose 2D Stamp Node - Copyright (C) 2015..2017 Walter Fetter Lages - - 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 - . - -*******************************************************************************/ - -#include - -#include - -#include -#include - -class Pose2DStamp -{ - public: - Pose2DStamp(ros::NodeHandle node); - ~Pose2DStamp(void); - - private: - ros::NodeHandle node_; - - ros::Subscriber poseSubscriber; - ros::Publisher posePublisher; - - int seq; - std::string frame_id; - - void poseCB(const geometry_msgs::Pose2D::ConstPtr &pose); -}; - - -Pose2DStamp::Pose2DStamp(ros::NodeHandle node) -{ - node_=node; - - poseSubscriber=node_.subscribe("command",1000,&Pose2DStamp::poseCB,this); - posePublisher=node_.advertise("command_stamped",1000); - - seq=0; - - ros::param::get("~frame_id",frame_id); -} - -Pose2DStamp::~Pose2DStamp(void) -{ - poseSubscriber.shutdown(); - posePublisher.shutdown(); -} - -void Pose2DStamp::poseCB(const geometry_msgs::Pose2D::ConstPtr &pose) -{ - geometry_msgs::PoseStamped stamped; - stamped.header.stamp=ros::Time::now(); - stamped.header.frame_id=frame_id; - stamped.pose.position.x=pose->x; - stamped.pose.position.y=pose->y; - stamped.pose.position.z=0; - stamped.pose.orientation.x=0; - stamped.pose.orientation.y=0; - stamped.pose.orientation.z=sin(pose->theta/2.0); - stamped.pose.orientation.w=cos(pose->theta/2.0); - posePublisher.publish(stamped); -} - -int main(int argc,char* argv[]) -{ - ros::init(argc,argv,"pose2d_stamp"); - ros::NodeHandle node; - - Pose2DStamp pose2DStamp(node); - - ros::Rate loop(100); - ros::Time t0=ros::Time::now(); - while(ros::ok()) - { - ros::spinOnce(); - loop.sleep(); - } - return 0; -}