--- /dev/null
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
--- /dev/null
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
+# directories (or patterns, but directories should suffice) that should
+# be excluded from the distro. This is not the place to put things that
+# should be ignored everywhere, like "build" directories; that happens in
+# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
+# ready for inclusion in a distro.
+#
+# This list is combined with the list in rosbuild/rosbuild.cmake. Note
+# that CMake 2.6 may be required to ensure that the two lists are combined
+# properly. CMake 2.4 seems to have unpredictable scoping rules for such
+# variables.
+#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
+
+rosbuild_make_distribution(0.1.0)
--- /dev/null
+include $(shell rospack find mk)/cmake_stack.mk
\ No newline at end of file
--- /dev/null
+<stack>
+ <description brief="twil">TWIL</description>
+ <author>Maintained by Taiser Barros</author>
+ <license>GPL</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://ros.org/wiki/twil</url>
+ <depend stack="ros" />
+
+</stack>
--- /dev/null
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+
+# Build RobotSim Interface
+rosbuild_add_library(twil_control_gazebo src/robot_sim_twil.cpp)
--- /dev/null
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
--- /dev/null
+<package>
+ <description brief="twil_control_gazebo">
+
+ twil_control_gazebo
+
+ </description>
+ <author>Walter Fetter Lages</author>
+ <license>GPL</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://ros.org/wiki/twil_control_gazebo</url>
+
+ <depend package="ros_control_gazebo"/>
+ <depend package="ros_control_gazebo_plugin"/>
+
+ <depend package="twil_description"/>
+
+ <depend package="gazebo"/>
+
+ <export>
+ <ros_control_gazebo plugin="${prefix}/robot_sim_plugins.xml" />
+ </export>
+
+</package>
--- /dev/null
+<library path="lib/libtwil_control_gazebo">
+
+ <class
+ name="twil_control_gazebo/RobotSimTwil"
+ type="twil_control_gazebo::RobotSimTwil"
+ base_class_type="ros_control_gazebo::RobotSim">
+ <description>
+ A ROS/Gazebo interface Twil, exporting a joint_state_interface and a
+ joint_effort_interface.
+ </description>
+ </class>
+</library>
--- /dev/null
+#include <pluginlib/class_list_macros.h>
+
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+
+#include <ros_control_gazebo/robot_sim.h>
+
+#include <angles/angles.h>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+
+namespace twil_control_gazebo
+{
+
+ class RobotSimTwil:public ros_control_gazebo::RobotSim
+ {
+
+ unsigned int n_dof_;
+
+ hardware_interface::JointStateInterface js_interface_;
+ hardware_interface::EffortJointInterface ej_interface_;
+
+ std::vector<std::string> joint_name_;
+ std::vector<double> joint_position_;
+ std::vector<double> joint_velocity_;
+ std::vector<double> joint_effort_;
+ std::vector<double> joint_effort_command_;
+
+ std::vector<gazebo::physics::JointPtr> sim_joints_;
+
+ public:
+
+ RobotSimTwil(void):n_dof_(2),joint_name_(n_dof_),joint_position_(n_dof_),
+ joint_velocity_(n_dof_),joint_effort_(n_dof_),joint_effort_command_(n_dof_)
+ {
+
+ joint_name_[0]="left_wheel_joint";
+ joint_name_[1]="right_wheel_joint";
+
+ for(unsigned int j=0;j < n_dof_;j++)
+ {
+ joint_position_[j]=0.0;
+ joint_velocity_[j]=0.0;
+ joint_effort_[j]=0.0;
+
+ joint_effort_command_[j] = 0.0;
+
+ js_interface_.registerJoint(joint_name_[j],&joint_position_[j],&joint_velocity_[j],&joint_effort_[j]);
+ ej_interface_.registerJoint(js_interface_.getJointStateHandle(joint_name_[j]),&joint_effort_command_[j]);
+ }
+
+ registerInterface(&js_interface_);
+ registerInterface(&ej_interface_);
+ }
+
+
+ bool initSim(ros::NodeHandle nh,gazebo::physics::ModelPtr model)
+ {
+ for(unsigned int j=0;j < n_dof_;j++)
+ {
+ ROS_INFO_STREAM("Getting pointer to gazebo joint: " << joint_name_[j]);
+ gazebo::physics::JointPtr joint=model->GetJoint(joint_name_[j]);
+ if(joint) sim_joints_.push_back(joint);
+ else
+ {
+ ROS_ERROR_STREAM("This robot has a joint named \"" << joint_name_[j]
+ <<"\" which is not in the gazebo model.");
+ return false;
+ }
+ }
+ return true;
+ }
+
+ void readSim(ros::Time time,ros::Duration period)
+ {
+ for(unsigned int j=0; j < n_dof_;j++)
+ {
+// joint_position_[j]+=angles::shortest_angular_distance
+// (joint_position_[j],sim_joints_[j]->GetAngle(0).GetAsRadian());
+ joint_position_[j]=sim_joints_[j]->GetAngle(0).GetAsRadian();
+ joint_velocity_[j]=sim_joints_[j]->GetVelocity(0);
+// joint_effort_[j]=sim_joints_[j]->GetForce(0);
+ joint_effort_[j]=joint_effort_command_[j];
+ }
+ }
+
+ void writeSim(ros::Time time,ros::Duration period)
+ {
+ for(unsigned int j=0;j < n_dof_;j++) sim_joints_[j]->SetForce(0,joint_effort_command_[j]);
+ }
+
+ };
+}
+
+PLUGINLIB_DECLARE_CLASS(twil_control_gazebo,RobotSimTwil,twil_control_gazebo::RobotSimTwil,ros_control_gazebo::RobotSim)
--- /dev/null
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+rosbuild_add_library(${PROJECT_NAME} src/cart_linearizing_controller.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
--- /dev/null
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
--- /dev/null
+To publish v:
+
+rostopic pub /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray "data: [0.0, 0.04]"
--- /dev/null
+twil:
+
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
+
+ left_wheel_joint_effort_controller:
+ type: effort_controllers/JointEffortController
+ joint: left_wheel_joint
+
+ right_wheel_joint_effort_controller:
+ type: effort_controllers/JointEffortController
+ joint: right_wheel_joint
--- /dev/null
+twil:
+
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
+
+ cart_linearizing_controller:
+ type: twil_controllers/CartLinearizingController
+ joints:
+ - left_wheel_joint
+ - right_wheel_joint
--- /dev/null
+twil:
+
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
+
+ left_wheel_joint_velocity_controller:
+ type: effort_controllers/JointVelocityController
+ joint: left_wheel_joint
+ pid: {p: 0.0, i: 0.0, d: 0.0}
+
+ right_wheel_joint_velocity_controller:
+ type: effort_controllers/JointVelocityController
+ joint: right_wheel_joint
+ pid: {p: 0.0, i: 0.0, d: 0.0}
+
--- /dev/null
+#ifndef TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H
+#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H
+
+#include <cstddef>
+#include <vector>
+#include <string>
+
+#include <ros/node_handle.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <sensor_msgs/JointState.h>
+
+#include <kdl/frames.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+
+
+namespace twil_controllers
+{
+ class CartLinearizingController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+ {
+ public:
+ CartLinearizingController(void);
+ ~CartLinearizingController(void);
+
+ bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
+ void starting(const ros::Time& time);
+ void update(const ros::Time& time);
+
+ private:
+ ros::NodeHandle node_;
+ ros::Time last_time_;
+ hardware_interface::EffortJointInterface *robot_;
+ std::vector<hardware_interface::JointHandle> joints_;
+
+ void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command);
+ ros::Subscriber sub_command_;
+
+ KDL::ChainIdSolver_RNE *idsolver;
+
+ KDL::JntArray phi;
+ KDL::JntArray nu;
+ KDL::JntArray dnu;
+ KDL::JntArray torque;
+ std::vector<double> v;
+
+ KDL::Wrenches fext;
+
+ std::vector<double> wheelRadius;
+ double wheelBase;
+ };
+}
+#endif
--- /dev/null
+<launch>
+
+ <include file="$(find twil_description)/launch/twil_sim.launch"/>
+
+ <rosparam file="$(find twil_controllers)/config/linearizing_control.yaml" command="load"/>
+
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="/twil" args="joint_state_controller cart_linearizing_controller"/>
+
+ <!--node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /-->
+</launch>
--- /dev/null
+<launch>
+
+ <include file="$(find twil_description)/launch/twil_sim.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="left_wheel_joint_effort_controller right_wheel_joint_effort_controller joint_state_controller"/>
+</launch>
--- /dev/null
+<launch>
+
+ <include file="$(find twil_description)/launch/twil_sim.launch"/>
+
+ <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"/>
+</launch>
--- /dev/null
+<package>
+ <description brief="twil_controllers">
+
+ twil_controllers
+
+ </description>
+ <author>Walter Fetter Lages</author>
+ <license>GPL</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://ros.org/wiki/twil_controllers</url>
+ <depend package="joint_state_controller"/>
+ <depend package="effort_controllers"/>
+ <depend package="twil_control_gazebo"/>
+ <depend package="controller_interface"/>
+ <depend package="orocos_kdl"/>
+ <depend package="kdl_parser"/>
+
+ <export>
+ <controller_interface plugin="${prefix}/twil_controllers_plugins.xml"/>
+ </export>
+
+</package>
+
+
--- /dev/null
+#include <twil_controllers/cart_linearizing_controller.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::CartLinearizingController(void):
+ phi(0),nu(0),dnu(0),torque(0),v(0),fext(0),wheelRadius(0)
+ {
+ }
+
+ CartLinearizingController::~CartLinearizingController(void)
+ {
+ sub_command_.shutdown();
+ }
+
+ bool CartLinearizingController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
+ {
+ node_=n;
+ robot_=robot;
+
+ XmlRpc::XmlRpcValue joint_names;
+ if(!node_.getParam("joints",joint_names))
+ {
+ ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str());
+ return false;
+ }
+
+ if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
+ {
+ ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str());
+ return false;
+ }
+
+ for(int i=0; i < joint_names.size();i++)
+ {
+ XmlRpc::XmlRpcValue &name_value=joint_names[i];
+ if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
+ {
+ ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str());
+ return false;
+ }
+
+ hardware_interface::JointHandle j=robot->getJointHandle((std::string)name_value);
+ joints_.push_back(j);
+ v.push_back(0);
+ }
+ sub_command_ = node_.subscribe("command",1000,&CartLinearizingController::commandCB, this);
+
+ std::string robot_desc_string;
+ if(!node_.getParam("/robot_description",robot_desc_string))
+ {
+ ROS_ERROR("Could not find '/robot_description'.");
+ return false;
+ }
+
+ KDL::Tree tree;
+ if (!kdl_parser::treeFromString(robot_desc_string,tree))
+ {
+ ROS_ERROR("Failed to construct KDL tree.");
+ return false;
+ }
+
+ 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::starting(const ros::Time& time)
+ {
+ last_time_=time;
+ for(unsigned int i=0; i < joints_.size();i++) v[i]=0.0;
+ }
+
+ void CartLinearizingController::update(const ros::Time& time)
+ {
+ ros::Duration dt=time-last_time_;
+ last_time_=time;
+
+ for(unsigned int i=0;i < joints_.size();i++)
+ {
+ phi(i)=joints_[i].getPosition();
+ nu(i)=joints_[i].getVelocity();
+ }
+
+
+ dnu(0)=v[0]/wheelRadius[0]-v[1]*wheelBase/2.0/wheelRadius[0]; // left wheel
+ dnu(1)=v[0]/wheelRadius[1]+v[1]*wheelBase/2.0/wheelRadius[1]; // right wheel
+
+
+ for(unsigned int i=0;i < fext.size();i++) fext[i].Zero();
+
+ for(unsigned int i=0;i < joints_.size();i++)
+ std::cout << "phi=" << phi(i) << " nu=" << nu(i)
+ << " dnu=" << dnu(i) << std::endl;
+
+ // Compute linearization.
+ if(idsolver->CartToJnt(phi,nu,dnu,fext,torque) < 0) ROS_ERROR("KDL inverse dynamics solver failed.");
+
+ for(unsigned int i=0;i < joints_.size(); i++)
+ std::cout << "torque=" << torque(i) << std::endl;
+
+ // Apply torques
+ for(unsigned int i=0;i < joints_.size();i++) joints_[i].setCommand(torque(i));
+ }
+
+ void CartLinearizingController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command)
+ {
+ for(unsigned int i=0;i < command->data.size();i++) v[i]=command->data[i];
+ }
+}
+PLUGINLIB_DECLARE_CLASS(twil_controllers,CartLinearizingController,twil_controllers::CartLinearizingController,controller_interface::ControllerBase)
--- /dev/null
+<library path="lib/libtwil_controllers">
+
+ <class name="twil_controllers/CartLinearizingController" type="twil_controllers::CartLinearizingController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ The CartLinearizingController linearizes the Twil dynamic model. The
+ linearized inputs are linear and angular accelerations. It expects a
+ EffortJointInterface type of hardware interface.
+ </description>
+ </class>
+
+</library>
--- /dev/null
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
--- /dev/null
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
--- /dev/null
+<launch>
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find twil_description)/xacro/twil.urdf.xacro'" />
+ <node name="spawn_twil_object" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model twil" respawn="false" output="screen" />
+</launch>
--- /dev/null
+<launch>
+ <!-- Start Gazebo -->
+ <include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
+
+ <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="spawn_twil_object" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model twil" respawn="false" output="screen" />
+</launch>
--- /dev/null
+<?xml version="1.0"?>
+<launch>
+ <!-- Start Gazebo -->
+ <include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
+
+ <include file="$(find twil_description)/launch/twil_wam.launch"/>
+</launch>
--- /dev/null
+<package>
+ <description brief="twil_description">
+
+ twil_description
+
+ </description>
+ <author>Walter Fetter Lages</author>
+ <license>GPL</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://ros.org/wiki/twil_description</url>
+
+</package>
+
+
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="battery_bosch_12v" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="-2.856E-07 0.0012183 -0.001734" rpy="0 0 0" />
+ <mass value="15" />
+ <inertia ixx="0.0730739985187771" ixy="1.98707075079408E-07" ixz="-2.54224589859575E-07" iyy="0.107634158285368" iyz="0.00135890388070998" izz="0.106246326145496" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/battery_bosch_12v.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.2 0.2 0.2 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/battery_bosch_12v.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="castor_base" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="-0.018396 -6.7757E-19 0.049957" rpy="0 0 0" />
+ <mass value="0.41627" />
+ <inertia ixx="0.00064128" ixy="-1.9687E-21" ixz="0.00019015" iyy="0.00067742" iyz="-1.3315E-20" izz="0.00018951" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_base.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.64706 0.61961 0.58431 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_base.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_joint" type="continuous">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}" />
+ <child link="${name}" />
+ <axis xyz="0 0 1" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/White</material>
+ </gazebo>
+
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="castor_support" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="0 1.4861E-17 -0.025271" rpy="0 0 0" />
+ <mass value="1.1237" />
+ <inertia ixx="0.00090319" ixy="-1.2436E-19" ixz="1.4703E-20" iyy="0.00079089" iyz="6.5874E-21" izz="0.00075135" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_support.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.64706 0.61961 0.58431 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_support.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="castor_wheel" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="0.11047" />
+ <inertia ixx="4.8046E-05" ixy="0" ixz="0" iyy="8.3809E-05" iyz="0" izz="4.8046E-05" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_wheel.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.75294 0.75294 0.75294 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/castor_wheel.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_joint" type="continuous">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}" />
+ <child link="${name}" />
+ <axis xyz="0 1 0" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="chassis" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin
+ xyz="6.3955E-06 -2.1963E-17 0.27338" rpy="0 0 0" />
+ <mass value="6.4923" />
+ <inertia ixx="0.67525" ixy="0.0014553" ixz="-0.00017525" iyy="0.69058" iyz="-6.3289E-18" izz="0.28611" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/chassis.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.75294 0.75294 0.75294 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/chassis.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_joint" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}"/>
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/Gold</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="cpu" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="8.9093E-05 2.5829E-06 0.00012597" rpy="0 0 0" />
+ <mass value="11" />
+ <inertia ixx="0.126605876415377" ixy="5.44490292261203E-06" ixz="0.000338979478568545" iyy="0.238484767338505" iyz="9.68721363707629E-07" izz="0.153116603514266" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/cpu.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.79216 0.81961 0.93333 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/cpu.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_joint" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/Grey</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:property name="EUROCARD_M" value="0.1" />
+ <xacro:property name="EUROCARD_W" value="0.16" />
+ <xacro:property name="EUROCARD_L" value="0.0016" />
+ <xacro:property name="EUROCARD_H" value="0.1" />
+
+ <xacro:macro name="eurocard" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="${EUROCARD_M}" />
+ <inertia ixx="${EUROCARD_M/12*(EUROCARD_H*EUROCARD_H+EUROCARD_L*EUROCARD_L)}" ixy="0.0" ixz="0.0" iyy="${EUROCARD_M/12*(EUROCARD_W*EUROCARD_W+EUROCARD_H*EUROCARD_H)}" iyz="0.0" izz="${EUROCARD_M/12*(EUROCARD_L*EUROCARD_L+EUROCARD_W*EUROCARD_W)}" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="${EUROCARD_W} ${EUROCARD_L} ${EUROCARD_H}" />
+ </geometry>
+ <material name="">
+ <color rgba="0.0 0.7 0.0 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="${EUROCARD_W} ${EUROCARD_L} ${EUROCARD_H}" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/Green</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="fixed_wheel" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="0.4935" />
+ <inertia ixx="0.0007853" ixy="0" ixz="0" iyy="0.001527" iyz="0" izz="0.0007853" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/fixed_wheel.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.29412 0.29412 0.29412 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/fixed_wheel.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}_joint" type="continuous">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}" />
+ <child link="${name}" />
+ <axis xyz="0 1 0" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="fixed_wheel_support" params="name parent *origin">
+
+ <link name="${name}">
+ <inertial>
+ <origin xyz="-0.027091 0.049354 -0.038294" rpy="0 0 0" />
+ <mass value="2.6913" />
+ <inertia ixx="0.004937" ixy="-0.00032401" ixz="-0.0013024" iyy="0.0052545" iyz="0.00022821" izz="0.0022004" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/fixed_wheel_support.stl" />
+ </geometry>
+ <material name="">
+ <color rgba="0.75294 0.75294 0.75294 1" />/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <mesh filename="package://twil_description/meshes/fixed_wheel_support.stl" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="${name}" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}" />
+ </joint>
+
+ <gazebo reference="${name}">
+ <material>Gazebo/White</material>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
--- /dev/null
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ name="twil">
+
+<!--robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:xacro="http://ros.org/wiki/xacro"
+ name="twil"-->
+
+ <property name="M_PI" value="3.1415926535897931" />
+
+ <include filename="$(find twil_description)/xacro/chassis.urdf.xacro" />
+ <include filename="$(find twil_description)/xacro/fixed_wheel_support.urdf.xacro" />
+ <include filename="$(find twil_description)/xacro/fixed_wheel.urdf.xacro" />
+ <include filename="$(find twil_description)/xacro/castor_support.urdf.xacro" />
+ <include filename="$(find twil_description)/xacro/castor_base.urdf.xacro" />
+ <include filename="$(find twil_description)/xacro/castor_wheel.urdf.xacro" />
+ <include filename="$(find twil_description)/xacro/battery_bosch_12v.urdf.xacro" />
+ <include filename="$(find twil_description)/xacro/cpu.urdf.xacro" />
+ <include filename="$(find twil_description)/xacro/eurocard.urdf.xacro" />
+
+ <link name="twil_origin"/>
+
+ <xacro:chassis name="chassis" parent="twil_origin">
+ <origin xyz="0 0 0.172" rpy="0 0 0"/>
+ </xacro:chassis>
+
+ <xacro:fixed_wheel_support name="right_wheel_support" parent="chassis">
+ <origin xyz="0 -0.161 -0.002" rpy="0 0 ${M_PI}" />
+ </xacro:fixed_wheel_support>
+
+ <xacro:fixed_wheel name="right_wheel" parent="right_wheel_support">
+ <origin xyz="0 0 -0.095" rpy="0 0 0" />
+ </xacro:fixed_wheel>
+
+ <xacro:fixed_wheel_support name="left_wheel_support" parent="chassis">
+ <origin xyz="0 0.161 -0.002" rpy="0 0 0" />
+ </xacro:fixed_wheel_support>
+
+ <xacro:fixed_wheel name="left_wheel" parent="left_wheel_support">
+ <origin xyz="0 0.0 -0.095" rpy="0 0 0" />
+ </xacro:fixed_wheel>
+
+ <xacro:castor_support name="castor_support" parent="chassis">
+ <origin xyz="-0.2 0 -0.002" rpy="0 0 0" />
+ </xacro:castor_support>
+
+ <xacro:castor_base name="castor_base" parent="castor_support">
+ <origin xyz="0 0 -0.1325" rpy="0 0 0" />
+ </xacro:castor_base>
+
+ <xacro:castor_wheel name="castor_wheel" parent="castor_base">
+ <origin xyz="-0.04 0 0" rpy="0 0 0" />
+ </xacro:castor_wheel>
+
+ <xacro:battery_bosch_12v name="battery" parent="chassis">
+ <origin xyz="-0.15 0.0 ${0.0015+0.0875}" rpy="0.0 0.0 ${M_PI/2}" />
+ </xacro:battery_bosch_12v>
+
+ <xacro:eurocard name="fan" parent="chassis">
+ <origin xyz="0 0.2 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:eurocard name="motor_driver" parent="chassis">
+ <origin xyz="0 0.1 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:eurocard name="stepper_driver" parent="chassis">
+ <origin xyz="0 0.0 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:eurocard name="power_supply" parent="chassis">
+ <origin xyz="0 -0.1 ${0.0015+0.3+0.003+0.1/2}" rpy="0.0 0.0 0.0" />
+ </xacro:eurocard>
+
+ <xacro:cpu name="cpu" parent="chassis">
+ <origin xyz="0 0.0 ${0.0015+0.3+0.003+0.127+0.003+0.17}" rpy="0.0 0.0 0.0" />
+ </xacro:cpu>
+
+ <link name="chassis_top" />
+
+ <joint name="chassis_top_joint" type="fixed">
+ <parent link="chassis"/>
+ <child link="chassis_top" />
+ <origin xyz="0.0 0.0 0.787" rpy="0.0 0.0 0.0" />
+ </joint>
+
+ <gazebo>
+ <controller:ros_control_gazebo_plugin
+ name="ros_control"
+ plugin="$(find ros_control_gazebo_plugin)/lib/libros_control_gazebo_plugin.so">
+ <ns>twil</ns>
+ <robotSimType>twil_control_gazebo/RobotSimTwil</robotSimType>
+ <controlPeriod>0.001</controlPeriod>
+ </controller:ros_control_gazebo_plugin>
+ </gazebo>
+
+</robot>
--- /dev/null
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="twil">
+
+ <include filename="$(find twil_description)/xacro/twil.urdf.xacro" />
+
+ <include filename="$(find wam_description)/xacro/wam_bhand.urdf.xacro" />
+
+
+ <joint name="twil_wam" type="fixed">
+ <parent link="chassis_top"/>
+ <child link="wam_origin" />
+ <origin xyz="-0.220 -0.140 0.0" rpy="0.0 0.0 0.0" />
+ </joint>
+
+
+ <include filename="$(find twil_description)/xacro/castor_support.urdf.xacro" />
+
+ <xacro:castor_support name="castor_support_front" parent="chassis">
+ <origin xyz="0.2 0 -0.002" rpy="0 0 0" />
+ </xacro:castor_support>
+
+ <xacro:castor_base name="castor_base_front" parent="castor_support_front">
+ <origin xyz="0 0 -0.1325" rpy="0 0 0" />
+ </xacro:castor_base>
+
+ <xacro:castor_wheel name="castor_wheel_front" parent="castor_base_front">
+ <origin xyz="-0.04 0 0" rpy="0 0 0" />
+ </xacro:castor_wheel>
+
+
+</robot>
projects / twil.git / commitdiff