<!--
<run_depend>twil_gazebo_ros_control</run_depend>
-->
- <run_depend>twil_controllers</run_depend>
<run_depend>twil_ident</run_depend>
-<!--
- <run_depend>twil_trajectories</run_depend>
--->
<!-- The export tag contains other, unspecified, tags -->
<export>
## 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
+++ /dev/null
-# 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]
+/******************************************************************************
+ 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
+++ /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,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_;
-
- 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 <Eigen/Dense>
-#include <boost/scoped_ptr.hpp>
-
-#include <ros/node_handle.h>
-
-#include <std_msgs/Float64MultiArray.h>
-#include <geometry_msgs/Pose2D.h>
-#include <nav_msgs/Odometry.h>
-#include <tf/tfMessage.h>
-
-#include <hardware_interface/joint_command_interface.h>
-#include <controller_interface/controller.h>
-#include <realtime_tools/realtime_publisher.h>
-
-#include <twil_controllers/NonSmoothBackstepControllerStatus.h>
-
-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,const ros::Duration& duration);
-
- private:
- ros::NodeHandle node_;
- hardware_interface::EffortJointInterface *robot_;
- std::vector<hardware_interface::JointHandle> joints_;
-
- boost::scoped_ptr<realtime_tools::RealtimePublisher
- <twil_controllers::NonSmoothBackstepControllerStatus>
- > status_publisher_ ;
-
- boost::shared_ptr<realtime_tools::RealtimePublisher
- <nav_msgs::Odometry> > odom_publisher_;
- boost::shared_ptr<realtime_tools::RealtimePublisher
- <tf::tfMessage> > tf_odom_publisher_;
-
- ros::Subscriber sub_command_;
- ros::Subscriber sub_parameters_;
-
- Eigen::Matrix2d Ginv;
- Eigen::Matrix2d F;
-
- std::vector<double> wheelRadius;
- double wheelBase;
-
- Eigen::Vector3d x;
- Eigen::Vector3d xRef;
-
- Eigen::Vector2d eta;
-
- ros::Time lastSamplingTime;
-
- Eigen::Vector2d phi;
-
- std::vector<double> lambda;
- std::vector<double> gamma;
-
- void commandCB(const geometry_msgs::Pose2D::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"/>
-
- <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>
-
-</launch>
+++ /dev/null
-<launch>
- <arg name="paused" default="true"/>
- <arg name="headless" default="false"/>
- <arg name="use_sim_time" default="true"/>
- <arg name="wam" default="false"/>
-
- <remap from="/twil/joint_states" to="/joint_states" />
-
- <include file="$(find twil_description)/launch/gazebo.launch" >
- <arg name="paused" value="$(arg paused)"/>
- <arg name="headless" value="$(arg headless)"/>
- <arg name="use_sim_time" value="$(arg use_sim_time)"/>
- <arg name="wam" value="$(arg wam)"/>
- </include>
-
- <include file="$(find twil_controllers)/launch/nonsmooth_backstep.launch" />
-
-</launch>
+++ /dev/null
-<launch>
- <arg name="paused" default="true"/>
- <arg name="headless" default="false"/>
- <arg name="use_sim_time" default="true"/>
- <arg name="wam" default="false"/>
-
- <remap from="/twil/command" to="/twil/nonsmooth_backstep_controller/command" />
- <remap from="/twil/joint_states" to="/joint_states" />
-
- <include file="$(find twil_description)/launch/gazebo.launch" >
- <arg name="paused" value="$(arg paused)"/>
- <arg name="headless" value="$(arg headless)"/>
- <arg name="use_sim_time" value="$(arg use_sim_time)"/>
- <arg name="wam" value="$(arg wam)"/>
- </include>
-
- <include file="$(find twil_controllers)/launch/nonsmooth_backstep.launch" />
-
- <node name="eight_trajectory" pkg="twil_trajectories" type="eight_trajectory" respawn="false" output="screen" ns="twil" args="_x:=0.0 _y:=-0.5 _radius:=1.0 _ang_vel:=0.1"/>
-
- <node name="pose2d_stamp" pkg="twil_trajectories" type="pose2d_stamp" respawn="false" output="screen" ns="twil" args="_frame_id:=map" />
-
- <node pkg="tf2_ros" type="static_transform_publisher" name="odom_publisher" args="0 0 0 0 0 0 1 map odom" />
-
- <include file="$(find twil_description)/launch/display.launch" />
-
-</launch>
+++ /dev/null
-<launch>
- <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"/>
-</launch>
+++ /dev/null
-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
+++ /dev/null
-float64 range
-float64 angle
-float64 orientation
+++ /dev/null
-#!/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
+++ /dev/null
-#!/bin/bash
-
-rostopic pub -1 /twil/nonsmooth_backstep_controller/command geometry_msgs/Pose2D "{x: $1, y: $2, theta: $3}"
+/******************************************************************************
+ 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>
+++ /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->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)
+++ /dev/null
-#include <boost/assign.hpp>
-
-#include <pluginlib/class_list_macros.h>
-#include <kdl/frames.hpp>
-#include <kdl_parser/kdl_parser.hpp>
-
-#include <twil_controllers/nonsmooth_backstep_controller.h>
-
-#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<twil_controllers::NonSmoothBackstepControllerStatus>(node_,"status",1));
- status_publisher_->msg_.header.frame_id="odom";
-
- odom_publisher_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(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<tf::tfMessage>(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)
</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>
<arg name="wam" value="$(arg wam)" />
</include>
- <node pkg="tf2_ros" type="static_transform_publisher" name="mob_origin_publisher" args="0 0 0 0 0 0 1 map twil_origin" />
+ <node pkg="tf2_ros" type="static_transform_publisher" name="twil_origin_publisher" args="0 0 0 0 0 0 1 map twil_origin" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" args="_use_gui:=$(arg use_gui)" />
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
Shaft Length: 1
Shaft Radius: 0.05
Shape: Arrow
- Topic: /twil/command_stamped
+ Topic: /command_stamped
Value: true
Enabled: true
Global Options:
<?xml version="1.0"?>
-<robot xmlns:xacro="http://ros.org/wiki/xacro" name="twil">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="twil">
<property name="M_PI" value="3.1415926535897931" />
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" >
- <robotNamespace>/twil</robotNamespace>
+ <!--robotNamespace>/twil</robotNamespace-->
<!-- Custom plugin -->
<!-- robotSimType>twil_gazebo_ros_control/TwilRobotHWSim</robotSimType -->
<?xml version="1.0"?>
-<robot xmlns:xacro="http://ros.org/wiki/xacro" name="twil">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="twil">
<xacro:include filename="$(find twil_description)/xacro/twil.urdf.xacro" />
## 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}
)
<arg name="headless" default="false"/>
<arg name="use_sim_time" default="true"/>
- <remap from="/twil/left_wheel_joint_effort_controller/command" to="/twil/left_wheel_command"/>
- <remap from="/twil/right_wheel_joint_effort_controller/command" to="/twil/right_wheel_command"/>
+ <remap from="/left_wheel_joint_effort_controller/command" to="/left_wheel_command"/>
+ <remap from="/right_wheel_joint_effort_controller/command" to="/right_wheel_command"/>
- <remap from="/twil/ident/left_wheel_command" to="/twil/left_wheel_command"/>
- <remap from="/twil/ident/right_wheel_command" to="/twil/right_wheel_command"/>
+ <remap from="/ident/left_wheel_command" to="/left_wheel_command"/>
+ <remap from="/ident/right_wheel_command" to="/right_wheel_command"/>
<include file="$(find twil_description)/launch/gazebo.launch" >
<arg name="paused" value="$(arg paused)"/>
<launch>
<include file="$(find twil_controllers)/launch/joint_effort.launch" />
- <node name="ident" ns="twil" pkg="twil_ident" type="ident" output="screen" />
+ <node name="ident" pkg="twil_ident" type="ident" output="screen" />
</launch>
+/******************************************************************************
+ ROS twil_indet Package
+ Twil Dynamics Model
+ 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 <ros/ros.h>
#include <Eigen/Dense>
+++ /dev/null
-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)
+++ /dev/null
-/******************************************************************************
- ROS twil_trajectories Package
- Circle Path Class
- Copyright (C) 2015..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 CIRCLE_PATH_H
-#define CIRCLE_PATH_H
-
-#include <Eigen/Dense>
-
-/** Circle path
-* @author Walter Fetter Lages <w.fetter@ieee.org>
-*/
-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
+++ /dev/null
-/******************************************************************************
- ROS twil_trajectories Package
- Eigth Path Class
- Copyright (C) 2015..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 EIGHT_PATH_H
-#define EIGHT_PATH_H
-
-#include <twil_trajectories/circle_path.h>
-
-/** 8 path
-* @author Walter Fetter Lages <w.fetter@ieee.org>
-*/
-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
+++ /dev/null
-<?xml version="1.0"?>
-<package>
- <name>twil_trajectories</name>
- <version>2.0.0</version>
- <description>The twil_trajectories package</description>
-
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
- <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
-
-
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
- <license>GPLv3</license>
-
-
- <!-- Url tags are optional, but mutiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/twil_ident</url> -->
-
-
- <!-- Author tags are optional, mutiple are allowed, one per tag -->
- <!-- Authors do not have to be maintianers, but could be -->
- <!-- Example: -->
- <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
- <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
-
-
- <!-- The *_depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use run_depend for packages you need at runtime: -->
- <!-- <run_depend>message_runtime</run_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>eigen</build_depend>
- <build_depend>geometry_msgs</build_depend>
-
- <run_depend>eigen</run_depend>
- <run_depend>geometry_msgs</run_depend>
-
- <!-- The export tag contains other, unspecified, tags -->
- <export>
- <!-- You can specify that this package is a metapackage here: -->
- <!-- <metapackage/> -->
-
- <!-- Other tools can request additional information be placed here -->
-
- </export>
-</package>
+++ /dev/null
-/******************************************************************************
- ROS twil_trajectories Package
- Circle Path Class
- Copyright (C) 2015..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_trajectories/circle_path.h>
-
-#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;
-}
+++ /dev/null
-/******************************************************************************
- ROS twil_trajectories Package
- Eigth Path Class
- Copyright (C) 2015..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_trajectories/eight_path.h>
-
-#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);
-}
+++ /dev/null
-/******************************************************************************
- ROS twil_trajectories Package
- Eigth Trajectory Node
- Copyright (C) 2015..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 <ros/ros.h>
-#include <geometry_msgs/Pose2D.h>
-
-#include <twil_trajectories/eight_path.h>
-
-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<geometry_msgs::Pose2D>("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;
-}
+++ /dev/null
-/******************************************************************************
- ROS twil_trajectories Package
- Pose 2D Stamp Node
- Copyright (C) 2015..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 <ros/ros.h>
-
-#include <Eigen/Dense>
-
-#include <geometry_msgs/Pose2D.h>
-#include <geometry_msgs/PoseStamped.h>
-
-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<geometry_msgs::PoseStamped>("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;
-}
projects / twil.git / commitdiff