<buildtool_depend>catkin</buildtool_depend>
<run_depend>twil_description</run_depend>
+<!--
<run_depend>twil_gazebo_ros_control</run_depend>
- <run_depend>twil_ident</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>
<!-- Other tools can request additional information be placed here -->
</export>
-</package>
\ No newline at end of file
+</package>
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
+ roscpp
controller_interface
effort_controllers
+ kdl_parser
+ geometry_msgs
+ nav_msgs
+ tf
+ message_generation
)
find_package(cmake_modules REQUIRED)
# 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
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
-# add_message_files(
-# FILES
+add_message_files(
+ FILES
# Message1.msg
# Message2.msg
-# )
+ NonSmoothBackstepControllerStatus.msg
+ PosePolar.msg
+)
## Generate services in the 'srv' folder
# add_service_files(
# )
## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# std_msgs # Or other packages containing msgs
-# )
+generate_messages(
+ DEPENDENCIES
+ std_msgs # Or other packages containing msgs
+ geometry_msgs
+ nav_msgs
+)
###################################
## catkin specific configuration ##
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
-# INCLUDE_DIRS include
+ INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
- CATKIN_DEPENDS controller_interface
+ CATKIN_DEPENDS message_runtime
# DEPENDS system_lib
- DEPENDS eigen
)
###########
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
-# add_dependencies(twil_controllers_node twil_controllers_generate_messages_cpp)
+add_dependencies(twil_controllers twil_controllers_generate_messages_cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}
rostopic pub /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray "data: [0.1, 0.0]"
+To publish with nonsmmoth_backstep_controller:
+
+rostopic pub /twil/nonsmooth_backstep_controller/command geometry_ms/Pose2D '{x: 0.0, y: 0.0, theta: 0.0}'
+
To get time and pose:
rostopic echo -p /gazebo/model_states | awk '{FS=","; printf("%g %g %g %g\n",$1/1e9,$11,$12,2*atan2(sqrt($14^2+$15^2+$16^2),$17))}'
+# Watch-out: The indenting here is relevant to the semantic!
+
twil:
joint_state_controller:
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]
#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 <std_msgs/Float64MultiArray.h>
-#include <sensor_msgs/JointState.h>
+#include <realtime_tools/realtime_publisher.h>
-#include <Eigen/Dense>
+#include <twil_controllers/NonSmoothBackstepControllerStatus.h>
namespace twil_controllers
{
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_;
std::vector<double> wheelRadius;
double wheelBase;
- Eigen::Vector3d xi;
- Eigen::Vector3d xiRef;
+ Eigen::Vector3d x;
+ Eigen::Vector3d xRef;
Eigen::Vector2d eta;
- void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command);
+ ros::Time lastSamplingTime;
+
+ std::vector<double> lambda;
+ std::vector<double> gamma;
+
+ void commandCB(const geometry_msgs::Pose2D::ConstPtr &command);
void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command);
};
}
<launch>
<remap from="/twil/nonsmooth_backstep_controller/dynamic_parameters" to="/twil/dynamic_parameters"/>
-
- <arg name="paused" default="true"/>
-
- <include file="$(find twil_description)/launch/twil_sim.launch">
- <arg name="paused" value="$(arg paused)"/>
- </include>
<rosparam file="$(find twil_controllers)/config/nonsmooth_backstep_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="/twil" args="joint_state_controller nonsmooth_backstep_controller"/>
+ output="screen" ns="twil" args="joint_state_controller nonsmooth_backstep_controller"/>
- <node name="ident" ns="/twil" pkg="twil_ident" type="ident" output="screen">
+ <node name="ident" ns="twil" pkg="twil_ident" type="ident" output="screen">
<remap from="ident/dynamic_parameters" to="dynamic_parameters"/>
</node>
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
-
</launch>
<launch>
-
- <arg name="paused" default="true"/>
-
- <include file="$(find twil_description)/launch/twil_sim.launch">
- <arg name="paused" value="$(arg paused)"/>
- </include>
-
<rosparam file="$(find twil_controllers)/config/linearizing_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="/twil" args="joint_state_controller cart_linearizing_controller"/>
-
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="/twil" />
-
+ output="screen" ns="twil" args="joint_state_controller cart_linearizing_controller"/>
</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"/>
+
+ <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/joint_effort.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/nonsmooth_backstep_controller/command" to="/twil/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 _y:=0 _radius:=1.0 _ang_vel:=0.2" />
+
+ [B<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>
<launch>
-
- <arg name="paused" default="true"/>
-
- <include file="$(find twil_description)/launch/twil_sim.launch">
- <arg name="paused" value="$(arg paused)"/>
- </include>
-
<rosparam file="$(find twil_controllers)/config/effort_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="/twil" args="left_wheel_joint_effort_controller right_wheel_joint_effort_controller joint_state_controller"/>
-
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="/twil" />
-
+ output="screen" ns="twil" args="left_wheel_joint_effort_controller right_wheel_joint_effort_controller joint_state_controller"/>
</launch>
<launch>
-
- <arg name="paused" default="true"/>
-
- <include file="$(find twil_description)/launch/twil_sim.launch">
- <arg name="paused" value="$(arg paused)"/>
- </include>
-
<rosparam file="$(find twil_controllers)/config/velocity_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="/twil" args="joint_state_controller left_wheel_joint_velocity_controller right_wheel_joint_velocity_controller"/>
-
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
-
+ output="screen" ns="twil" args="joint_state_controller left_wheel_joint_velocity_controller right_wheel_joint_velocity_controller"/>
</launch>
<launch>
-
- <arg name="paused" default="true"/>
-
- <include file="$(find twil_description)/launch/twil_sim.launch">
- <arg name="paused" value="$(arg paused)"/>
- </include>
-
<rosparam file="$(find twil_controllers)/config/nonsmooth_backstep_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="/twil" args="joint_state_controller nonsmooth_backstep_controller"/>
-
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
-
+ 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
<build_depend>controller_interface</build_depend>
<build_depend>effort_controllers</build_depend>
<build_depend>kdl_parser</build_depend>
- <build_depend>orocos_kdl</build_depend>
- <build_depend>twil_gazebo_ros_control</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>nav_msgs</build_depend>
+ <build_depend>tf</build_depend>
+ <build_depend>control_toolbox</build_depend>
+ <build_depend>realtime_tools</build_depend>
+ <build_depend>message_generation</build_depend>
<run_depend>controller_interface</run_depend>
<run_depend>controller_manager</run_depend>
<run_depend>effort_controllers</run_depend>
<run_depend>joint_state_controller</run_depend>
<run_depend>kdl_parser</run_depend>
- <run_depend>orocos_kdl</run_depend>
- <run_depend>twil_gazebo_ros_control</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>nav_msgs</run_depend>
+ <run_depend>tf</run_depend>
+ <run_depend>control_toolbox</run_depend>
+ <run_depend>realtime_tools</run_depend>
+ <run_depend>message_runtime</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<!-- Other tools can request additional information be placed here -->
<controller_interface plugin="${prefix}/twil_controllers_plugins.xml"/>
</export>
-</package>
\ No newline at end of file
+</package>
--- /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=100)
+
+ rospy.init_node('pose_step',anonymous=True)
+
+ 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}"
-#include <twil_controllers/nonsmooth_backstep_controller.h>
-#include <pluginlib/class_list_macros.h>
+#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)
+// Are numeric derivates of eta too noisy?
+// It is not simple to compute it analitically...
#define NUMERICAL_DETA
namespace twil_controllers
{
NonSmoothBackstepController::NonSmoothBackstepController(void):
- wheelRadius(2)
+ 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)
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;
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]=1;
+ gamma[1]=1;
+ gamma[2]=1;
+ gamma[3]=1;
+ node_.param("gamma",gamma,gamma);
+
+ lambda[0]=1;
+ lambda[1]=1;
+ lambda[2]=1;
+ lambda[3]=1;
+ lambda[4]=1;
+ node_.param("lambda",lambda,lambda);
- const double K5 = 0.0018533548425194695;
- const double K6 = 0.09946140462774823;
- const double K7 = 21.65458426501294;
- const double K8 = -15.40102896939387;
- Ginv << 0.5*K7, 0.5*K8,
- 0.5*K7, -0.5*K8;
+ 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;
void NonSmoothBackstepController::starting(const ros::Time& time)
{
- xi.setZero();
- xiRef.setZero();
+ x.setZero();
+ xRef.setZero();
eta.setZero();
-
+ lastSamplingTime=time;
}
void NonSmoothBackstepController::update(const ros::Time& time,
const ros::Duration& duration)
{
- if(duration.toSec() < 0.01) return;
-
- Eigen::Vector2d nu;
+ double dt=time.toSec()-lastSamplingTime.toSec();
+
+ if(fabs(dt-0.01) > 0.001/2) return;
+ lastSamplingTime=time;
+
+ Eigen::Vector2d w;
for(unsigned int i=0;i < joints_.size();i++)
{
- nu[i]=joints_[i].getVelocity();
+ w[i]=joints_[i].getVelocity();
}
-
+
+ // w[0] is the left wheel velocity
+ // w[1] is the right wheel velocity
Eigen::Vector2d u;
- u[0]=(nu[0]*wheelRadius[0]+nu[1]*wheelRadius[1])/2.0;
- u[1]=(nu[1]*wheelRadius[1]-nu[0]*wheelRadius[0])/wheelBase;
-
+ u[0]=(w[0]*wheelRadius[0]+w[1]*wheelRadius[1])/2.0;
+ u[1]=(w[1]*wheelRadius[1]-w[0]*wheelRadius[0])/wheelBase;
+
// Estimate pose by odometry
Eigen::MatrixXd B(3,2);
- B << cos(xi[2]+u[1]*duration.toSec()/2.0), 0.0,
- sin(xi[2]+u[1]*duration.toSec()/2.0), 0.0,
+ double deltaTheta2=u[1]*dt/2.0;
+ double deltaS=(fabs(deltaTheta2) > 1e-10)? sin(deltaTheta2)/deltaTheta2:1.0;
+
+ B << cos(x[2]+deltaS*deltaTheta2), 0.0,
+ sin(x[2]+deltaS*deltaTheta2), 0.0,
0.0, 1.0;
-
- xi+=B*u*duration.toSec();
-
+
+ Eigen::Vector3d dx=B*u;
+
+ x+=dx*dt;
+
// Change of coordinates
Eigen::Matrix3d R;
- R << cos(xiRef[2]), sin(xiRef[2]), 0.0,
- -sin(xiRef[2]), cos(xiRef[2]), 0.0,
+ 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*(xi-xiRef);
+ 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;
- // Backstep controller
- const double GAMMA1=10.0;
- const double GAMMA2=1.0;
-// const double GAMMA3=1.0;
- const double GAMMA4=10.0;
- const double GAMMA5=1.0;
- const double LAMBDA1=1.0;
- const double LAMBDA2=0.1;
- const double LAMBDA3=0.1;
- const double LAMBDA4=10.0;
- const double LAMBDA5=100.0;
-
#ifdef NUMERICAL_DETA
Eigen::Vector2d deta=-eta;
#endif
- Eigen::Vector2d eta;
-
- eta[0]=-GAMMA1*e*cos(alpha);
+ eta[0]=-gamma[0]*e*cos(alpha);
- if(alpha > 1e-6) eta[1]=-GAMMA2*alpha-GAMMA1*sin(alpha)*cos(alpha)+GAMMA1*LAMBDA3*psi*sin(alpha)/LAMBDA2/alpha*cos(alpha);
- else eta[1]=GAMMA1*LAMBDA3*psi/LAMBDA2;
+ 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(e > 1e-6) vBar[0]=-GAMMA4*eb[0]-LAMBDA1/LAMBDA4*cos(alpha)
- +LAMBDA2/LAMBDA4*alpha*sin(alpha)/e
- -LAMBDA3/LAMBDA4*psi*sin(alpha)/e;
- else vBar[0]=-GAMMA4*eb[0]-LAMBDA1/LAMBDA4*cos(alpha);
- vBar[1]=-GAMMA5*eb[1]-LAMBDA2/LAMBDA5*alpha;
-
-#ifndef NUMERICAL_DETA
+ 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;
+
+#ifdef NUMERICAL_DETA
+ // Numerical derivates
+ deta+=eta;
+ deta/=dt;
+#else
// Analitic derivates
Eigen::Vector2d deta;
- deta[0]=sqr(GAMMA1)*e*cub(cos(alpha))-GAMMA1*GAMMA2*e*alpha*sin(alpha)
- +sqr(GAMMA1)*LAMBDA3/LAMBDA2*e*cos(alpha)*sqr(sin(alpha))/alpha*psi;
- deta[1]=sqr(GAMMA2)*alpha-2.0*GAMMA1*GAMMA2*LAMBDA3/LAMBDA2*psi*sin(alpha)/alpha*cos(alpha)
- +GAMMA1*GAMMA2*alpha*sqr(cos(alpha))-sqr(GAMMA1)*LAMBDA3/LAMBDA2*cub(cos(alpha))*sin(alpha)/alpha*psi
- -GAMMA1*GAMMA2*alpha*sqr(sin(alpha))+sqr(GAMMA1)*LAMBDA3/LAMBDA2*cos(alpha)*cub(sin(alpha))/alpha*psi
- -sqr(GAMMA1)*LAMBDA3/LAMBDA2*sqr(cos(alpha))*sqr(sin(alpha))/alpha-GAMMA1*GAMMA2*LAMBDA3/LAMBDA2*psi
- +sqr(GAMMA1*LAMBDA3/LAMBDA2)*cub(cos(alpha))*sin(alpha)/sqr(alpha)*sqr(psi)+sqr(GAMMA1*LAMBDA3/LAMBDA2)*cos(alpha)*cub(sin(alpha))/sqr(alpha)*sqr(psi)
- +sqr(GAMMA1*LAMBDA3/LAMBDA2*cos(alpha)*sin(alpha)*psi)/cub(alpha);
-#else
- deta+=eta;
- deta/=duration.toSec();
+ deta[0]=sqr(gamma[0])*e*cub(cos(alpha))-gamma[0]*gamma[1]*e*alpha*sin(alpha)
+ +sqr(gamma[0])*lambda[2]/lambda[1]*e*cos(alpha)*sqr(sin(alpha))/alpha*psi;
+ deta[1]=sqr(gamma[1])*alpha-2.0*gamma[0]*gamma[1]*lambda[2]/lambda[1]*psi*sin(alpha)/alpha*cos(alpha)
+ +gamma[0]*gamma[1]*alpha*sqr(cos(alpha))-sqr(gamma[0])*lambda[2]/lambda[1]*cub(cos(alpha))*sin(alpha)/alpha*psi
+ -gamma[0]*gamma[1]*alpha*sqr(sin(alpha))+sqr(gamma[0])*lambda[2]/lambda[1]*cos(alpha)*cub(sin(alpha))/alpha*psi
+ -sqr(gamma[0])*lambda[2]/lambda[1]*sqr(cos(alpha))*sqr(sin(alpha))/alpha-gamma[0]*gamma[1]*lambda[2]/lambda[1]*psi
+ +sqr(gamma[0]*lambda[2]/lambda[1])*cub(cos(alpha))*sin(alpha)/sqr(alpha)*sqr(psi)+sqr(gamma[0]*lambda[2]/lambda[1])*cos(alpha)*cub(sin(alpha))/sqr(alpha)*sqr(psi)
+ +sqr(gamma[0]*lambda[2]/lambda[1]*cos(alpha)*sin(alpha)*psi)/cub(alpha);
#endif
-
+
Eigen::Vector2d v=vBar+deta;
-
// Linearization.
Eigen::Vector2d uf;
uf << u[0]*u[1], sqr(u[1]);
-
- Eigen::Vector2d torque=Ginv*(v+F*uf);
+
+ Eigen::Vector2d torque=Ginv*(v-F*uf);
// Apply torques
for(unsigned int i=0;i < joints_.size();i++)
{
- if(torque[i] > 100.0) torque[i]=100.0;
- if(torque[i] < -100.0) torque[i]=-100.0;
joints_[i].setCommand(torque[i]);
}
+
+ 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=dx[0];
+ status_publisher_->msg_.process_value_dot.y=dx[1];
+ status_publisher_->msg_.process_value_dot.theta=dx[2];
+
+ 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=dx[0];
+ odom_publisher_->msg_.twist.twist.linear.y=dx[1];
+ odom_publisher_->msg_.twist.twist.angular.z=dx[2];
+
+ 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 std_msgs::Float64MultiArray::ConstPtr &command)
+ void NonSmoothBackstepController::commandCB(const geometry_msgs::Pose2D::ConstPtr &command)
{
- for(unsigned int i=0;i < command->data.size() && i < 2;i++) xiRef[i]=command->data[i];
+ xRef[0]=command->x;
+ xRef[1]=command->y;
+ xRef[2]=command->theta;
}
- void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command)
+ 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.5K7 0.5K8
- // 0.5K7 -0.5K8
+ // Ginv=[0.5/K7 0.5/K8
+ // 0.5/K7 -0.5/K8]
- if(command->data[4] < 1e-3) F(0,1)=command->data[0];
- if(command->data[5] < 1e-3) F(1,0)=command->data[1];
- if(command->data[6] < 1e-3)
+ 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*command->data[2];
- Ginv(1,0)=0.5*command->data[2];
+ Ginv(0,0)=0.5/param->data[2];
+ Ginv(1,0)=0.5/param->data[2];
}
- if(command->data[7] < 1e-3)
+ if(param->data[7] < 1e-3)
{
- Ginv(0,1)=0.5*command->data[3];
- Ginv(1,1)=-0.5*command->data[3];
+ Ginv(0,1)=0.5/param->data[3];
+ Ginv(1,1)=-0.5/param->data[3];
}
}
--- /dev/null
+here are a couple of sources of undesired "bouncing behavior" that I've
+commonly seen with vehicles:
+
+ cause: interpenetration correction applies an impulse that pushes the
+object away from interpenetrating collision, but if the impulse is too large
+it causes the object to "bounce/fly away". fix: set maximum velocity
+impulse (max_vel) to 0 will enforce pure position correction with no added
+momentum from interpenetration correction. It is recommended to set a
+finite minimum allowable interpenetration depth (min_depth) of 1mm or
+something small in comparison to the colliding objects.
+
+ cause: controller exerts unrealistically large efforts on joints or
+links. fix: double check the forces/torques commanded by your controller
+and make sure they are physically realistic if you are looking for
+physically realistic responses.
+
+ cause: unrealistically high velocity / momentum buildup. fix: add
+dissipation by adding joint damping and set implicit_spring_damper flag to
+true to avoid numerical instability. Joint damping value is in the units of
+(effort / velocity). One extremely simplistic way to try and come up with
+viscous damping estimates is this: at what joint velocity do you want
+significant effort opposing your actuation forces? Assuming for example, we
+are controlling a hinged joint, and the actuator is capable of putting out 1
+Nm of torque. A damping coefficient of 2 means the damping force opposing
+your actuator will neutralize actuation torque of 1 Nm when the joint is
+rotating at 0.5 rad/s.
+
+ cause: friction coefficients are too large, combined with failure of
+dynamics solver to resolve all constraints sufficiently in allotted time.
+This is especially true of you have opposing frictional forces at work such
+as in the cases of skid-steer or non-ackerman steering. fix: use friction
+coefficient of 1 or lower, and potentially increase number of inner
+iterations.
+
+Here's an example for specifying contact parameters discussed above in URDF
+as Gazebo extensions, this example applies the parameters to all collision
+shapes specified under a link named my_link:
+
+<gazebo reference="my_link">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>0.8</mu1>
+ <mu2>0.8</mu2>
+ <maxVel>0.0</maxVel>
+ <minDepth>0.001</minDepth>
+</gazebo>
+
--- /dev/null
+<launch>
+ <arg name="wam" default="false"/>
+ <arg name="use_gui" default="false"/>
+
+ <include file="$(find twil_description)/launch/twil.launch" >
+ <arg name="wam" value="$(arg wam)" />
+ </include>
+
+ <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" args="_use_gui:=$(arg use_gui)" />
+
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(find twil_description)/rviz/urdf.rviz" required="true"/>
+</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"/>
+
+ <include file="$(find gazebo_ros)/launch/empty_world.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="world_name" value="worlds/empty_sky.world" />
+ </include>
+
+ <include file="$(find twil_description)/launch/twil.launch" >
+ <arg name="wam" value="$(arg wam)" />
+ </include>
+
+ <node name="twil_spawner" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model twil" respawn="false" output="screen" />
+</launch>
<launch>
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find twil_description)/xacro/twil.urdf.xacro'" />
- <node name="twil_spawner" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model twil" respawn="false" output="screen" />
+ <arg name="wam" default="false"/>
+
+ <param unless="$(arg wam)" name="robot_description" command="$(find xacro)/xacro.py '$(find twil_description)/xacro/twil.urdf.xacro'" />
+ <param if="$(arg wam)" name="robot_description" command="$(find xacro)/xacro.py '$(find twil_description)/xacro/twil_wam.urdf.xacro'" />
</launch>
+++ /dev/null
-<launch>
-
- <arg name="paused" default="false"/>
-
- <!-- Start Gazebo -->
- <include file="$(find gazebo_ros)/launch/empty_world.launch">
- <arg name="paused" value="$(arg paused)"/>
- <arg name="world_name" value="worlds/empty_sky.world" />
- </include>
-
- <include file="$(find twil_description)/launch/twil.launch"/>
-</launch>
+++ /dev/null
-<launch>
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find twil_description)/xacro/twil_wam.urdf.xacro'" />
- <node name="twil_spawner" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model twil" respawn="false" output="screen" />
-</launch>
+++ /dev/null
-<?xml version="1.0"?>
-<launch>
-
- <arg name="paused" default="false"/>
-
- <!-- Start Gazebo -->
- <include file="$(find gazebo_ros)/launch/empty_world.launch">
- <arg name="paused" value="$(arg paused)"/>
- <arg name="world_name" value="worlds/empty_sky.world" />
- </include>
-
- <include file="$(find twil_description)/launch/twil_wam.launch"/>
-</launch>
--- /dev/null
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /Odometry1
+ - /Pose1
+ Splitter Ratio: 0.5
+ Tree Height: 559
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: true
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.03
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ battery:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ castor_base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ castor_support:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ castor_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ chassis:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ chassis_top:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ cpu:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fan:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_castor_base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_castor_support:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_castor_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_wheel_support:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ motor_driver:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ power_supply:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_wheel_support:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ stepper_driver:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ twil_origin:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Angle Tolerance: 0.1
+ Class: rviz/Odometry
+ Color: 255; 25; 0
+ Enabled: true
+ Keep: 42
+ Length: 1
+ Name: Odometry
+ Position Tolerance: 0.3
+ Topic: /twil/nonsmooth_backstep_controller/odom
+ Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.1
+ Class: rviz/Pose
+ Color: 85; 0; 255
+ Enabled: true
+ Head Length: 0.3
+ Head Radius: 0.1
+ Name: Pose
+ Shaft Length: 1
+ Shaft Radius: 0.05
+ Shape: Arrow
+ Topic: /twil/command_stamped
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 7.48193
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.06
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Name: Current View
+ Near Clip Distance: 0.01
+ Pitch: 0.785398
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 0.785398
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 846
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000013c000002bafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000036000002ba000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000036000002ba0000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002c200fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002ba00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1200
+ X: 60
+ Y: 60
<gazebo reference="${name}">
<material>Gazebo/Grey</material>
+<!--
+ <mu1>1</mu1>
+ <mu2>1</mu2>
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <minDepth>0.001</minDepth>
+ <maxVel>1.0</maxVel>
+ <fdir1>1 0 0</fdir1>
+-->
</gazebo>
</xacro:macro>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <property name="M_PI" value="3.1415926535897931" />
+
<xacro:macro name="fixed_wheel" params="name parent *origin">
<link name="${name}">
<parent link="${parent}" />
<child link="${name}" />
<axis xyz="0 1 0" />
+ <limit effort="100.0" velocity="${2*M_PI}" />
</joint>
<transmission name="${name}_transmission">
<gazebo reference="${name}">
<material>Gazebo/FlatBlack</material>
+<!--
+ <mu1>100000.0</mu1>
+ <mu2>100000.0</mu2>
+ <kp>500000.0</kp>
+ <kd>10.0</kd>
+ <minDepth>0.001</minDepth>
+ <maxVel>0.1</maxVel>
+ <fdir1>1 0 0</fdir1>
+-->
</gazebo>
</xacro:macro>
## 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(catkin REQUIRED COMPONENTS roscpp kdl_parser)
find_package(cmake_modules REQUIRED)
--- /dev/null
+<launch>
+ <arg name="paused" default="true"/>
+ <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="/twil/ident/left_wheel_command" to="/twil/left_wheel_command"/>
+ <remap from="/twil/ident/right_wheel_command" to="/twil/right_wheel_command"/>
+
+ <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)"/>
+ </include>
+
+ <include file="$(find twil_ident)/launch/ident.launch"/>
+</launch>
<launch>
- <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"/>
-
- <include file="$(find twil_description)/launch/twil_sim.launch"/>
+ <include file="$(find twil_controllers)/launch/joint_effort.launch" />
- <rosparam file="$(find twil_controllers)/config/effort_control.yaml" command="load"/>
-
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="/twil"
- args="joint_state_controller left_wheel_joint_effort_controller right_wheel_joint_effort_controller"/>
-
- <node name="ident" ns="/twil" pkg="twil_ident" type="ident" output="screen">
- <remap from="ident/left_wheel_command" to="left_wheel_command"/>
- <remap from="ident/right_wheel_command" to="right_wheel_command"/>
- </node>
+ <node name="ident" ns="twil" pkg="twil_ident" type="ident" output="screen" />
</launch>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
- <maintainer email="fetter@eceufrgs.br">Walter Fetter Lages</maintainer>
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>eigen</build_depend>
+ <build_depend>kdl_parser</build_depend>
<run_depend>eigen</run_depend>
-
+ <run_depend>kdl_parser</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/JointState.h>
+#include <kdl_parser/kdl_parser.hpp>
+
+
class Prbs
{
int index;
ros::Time lastTime;
+ std::vector<double> wheelRadius;
+ double wheelBase;
+
void jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates);
void resetCovariance(void);
};
Ident::Ident(ros::NodeHandle node):
- nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),prbs(nJoints)
+ nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),prbs(nJoints),wheelRadius(nJoints)
{
node_=node;
- jointStatesSubscriber=node_.subscribe("/joint_states",1000,&Ident::jointStatesCB,this);
+ std::string robot_desc_string;
+ if(!node_.getParam("/robot_description",robot_desc_string))
+ {
+ ROS_ERROR("Could not find '/robot_description'.");
+ }
+
+ KDL::Tree tree;
+ if (!kdl_parser::treeFromString(robot_desc_string,tree))
+ {
+ ROS_ERROR("Failed to construct KDL tree.");
+ }
+
+ // 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(nJoints);
+ 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();
+
+ jointStatesSubscriber=node_.subscribe("joint_states",1000,&Ident::jointStatesCB,this);
dynParamPublisher=node_.advertise<std_msgs::Float64MultiArray>("ident/dynamic_parameters",1000);
leftWheelCommandPublisher=node_.advertise<std_msgs::Float64>("ident/left_wheel_command",1000);
rightWheelCommandPublisher=node_.advertise<std_msgs::Float64>("ident/right_wheel_command",1000);
Phi2[0]=u[0]*u[1]; // u1(k)*u2(k)
Eigen::VectorXd torque(nJoints);
+
+ // u(k+1)
+ // jointStates->velocity[0] is left wheel
+ // jointStates->velocity[1] is right wheel
+ u[0]=(jointStates->velocity[0]*wheelRadius[0]+jointStates->velocity[1]*wheelRadius[1])/2;
+ u[1]=(jointStates->velocity[1]*wheelRadius[1]-jointStates->velocity[0]*wheelRadius[0])/wheelBase;
+
for(int i=0;i < nJoints;i++)
- {
- u[i]=jointStates->velocity[i]; // u(k+1)
torque[i]=jointStates->effort[i]; // torque(k)
- }
y+=u;
y/=dt.toSec();
P2-=K2*Phi2.transpose()*P2;
std_msgs::Float64MultiArray dynParam;
+ dynParam.layout.dim.push_back(std_msgs::MultiArrayDimension());
+ dynParam.layout.dim[0].label="K5 K6 K7 K8 P55 P66 P77 P88";
+ dynParam.layout.dim[0].size=nJoints*4;
+ dynParam.layout.dim[0].stride=1;
+ dynParam.layout.data_offset=0;
for(int i=0;i < nJoints;i++)
{
dynParam.data.push_back(thetaEst1[i]);
--- /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
+#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
+#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
+#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
+#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
+#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
+#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