saved work

This commit is contained in:
Ruixiang Du
2020-04-01 18:51:34 +08:00
parent b8144488f3
commit d221d37dea
9 changed files with 344 additions and 56 deletions

12
NOTE.md
View File

@@ -1,3 +1,15 @@
```
$ rostopic pub -1 /scout_motor_fl_controller/command std_msgs/Float64 "data: 0.5"
```
Convert xacro to urdf
```
$ rosrun xacro xacro -o model.urdf model.urdf.xacro
```
Convert urdf to sdf
```
$ gz sdf -p scout_v2.urdf > scout_v2.sdf
```

View File

@@ -0,0 +1,277 @@
<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from scout_v2.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="scout_v2">
<!-- Virtual link as root -->
<link name="vehicle_reference"/>
<joint name="veh_ref_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="vehicle_reference"/>
<child link="base_link"/>
</joint>
<!-- Base link -->
<link name="base_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://scout_description/meshes/base_link.dae"/>
</geometry>
</visual>
<inertial>
<mass value="40"/>
<!-- <mass value="50" /> -->
<!-- <origin xyz="-0.00065 -0.085 0.062" />
<inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296" /> -->
<origin xyz="0.0 0.0 0.0"/>
<inertia ixx="2.288641" ixy="0" ixz="0" iyy="5.103976" iyz="0" izz="3.431465"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.008"/>
<geometry>
<box size="0.925 0.38 0.21"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.035"/>
<geometry>
<box size="0.154166666667 0.627 0.07"/>
</geometry>
</collision>
</link>
<!-- Base footprint: base_link projected to the gound plane -->
<link name="base_footprint"/>
<joint name="base_footprint_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.23479"/>
<parent link="base_link"/>
<child link="base_footprint"/>
</joint>
<link name="front_right_wheel_link">
<inertial>
<mass value="3"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://scout_description/meshes/wheel_type1.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.11653" radius="0.16459"/>
</geometry>
</collision>
</link>
<joint name="front_right_wheel" type="continuous">
<parent link="base_link"/>
<child link="front_right_wheel_link"/>
<origin rpy="3.14 0 0" xyz="0.249 -0.29153 -0.0702"/>
<axis rpy="0 0 0" xyz="0 -1 0"/>
</joint>
<link name="front_left_wheel_link">
<inertial>
<mass value="3"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://scout_description/meshes/wheel_type2.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.11653" radius="0.16459"/>
</geometry>
</collision>
</link>
<joint name="front_left_wheel" type="continuous">
<parent link="base_link"/>
<child link="front_left_wheel_link"/>
<origin rpy="0 0 0" xyz="0.249 0.29153 -0.0702"/>
<axis rpy="0 0 0" xyz="0 -1 0"/>
</joint>
<link name="rear_left_wheel_link">
<inertial>
<mass value="3"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://scout_description/meshes/wheel_type1.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.11653" radius="0.16459"/>
</geometry>
</collision>
</link>
<joint name="rear_left_wheel" type="continuous">
<parent link="base_link"/>
<child link="rear_left_wheel_link"/>
<origin rpy="0 0 0" xyz="-0.249 0.29153 -0.0702"/>
<axis rpy="0 0 0" xyz="0 -1 0"/>
</joint>
<link name="rear_right_wheel_link">
<inertial>
<mass value="3"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://scout_description/meshes/wheel_type2.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.11653" radius="0.16459"/>
</geometry>
</collision>
</link>
<joint name="rear_right_wheel" type="continuous">
<parent link="base_link"/>
<child link="rear_right_wheel_link"/>
<origin rpy="3.14 0 0" xyz="-0.249 -0.29153 -0.0702"/>
<axis rpy="0 0 0" xyz="0 -1 0"/>
</joint>
<!-- For testing, hang the robot up in the air -->
<!-- <link name="world" />
<joint name="world_to_base_link=" type="fixed">
<origin xyz="0 0 0.5" rpy="0 0 0" />
<parent link="world"/>
<child link="base_link"/>
</joint> -->
<!-- This file is a placeholder which is included by default from
husky.urdf.xacro. If a robot is being customized and requires
additional URDF, set the HUSKY_URDF_EXTRAS environment variable
to the full path of the file you would like included. -->
<!-- Additional definitions for simulation -->
<!-- <gazebo reference="base_link">
<material>Gazebo/Grey</material>
</gazebo> -->
<!-- kp spring constant, kd damping constant -->
<gazebo reference="front_right_wheel_link">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<fdir1 value="1 0 0"/>
<minDepth value="0.001"/>
<maxContacts value="64"/>
</gazebo>
<transmission name="front_right_wheel_trans" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="front_right_wheel_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="front_right_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
</transmission>
<!-- kp spring constant, kd damping constant -->
<gazebo reference="front_left_wheel_link">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<fdir1 value="1 0 0"/>
<minDepth value="0.001"/>
<maxContacts value="64"/>
</gazebo>
<transmission name="front_left_wheel_trans" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="front_left_wheel_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="front_left_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
</transmission>
<!-- kp spring constant, kd damping constant -->
<gazebo reference="rear_left_wheel_link">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<fdir1 value="1 0 0"/>
<minDepth value="0.001"/>
<maxContacts value="64"/>
</gazebo>
<transmission name="rear_left_wheel_trans" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="rear_left_wheel_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="rear_left_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
</transmission>
<!-- kp spring constant, kd damping constant -->
<gazebo reference="rear_right_wheel_link">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<fdir1 value="1 0 0"/>
<minDepth value="0.001"/>
<maxContacts value="64"/>
</gazebo>
<transmission name="rear_right_wheel_trans" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="rear_right_wheel_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="rear_right_wheel">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
</transmission>
<!-- Controller configurations -->
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
<robotNamespace>/</robotNamespace>
</plugin>
</gazebo>
<!-- <gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<updateRate>100.0</updateRate>
<robotNamespace></robotNamespace>
<leftFrontJoint>front_left_wheel</leftFrontJoint>
<rightFrontJoint>front_right_wheel</rightFrontJoint>
<leftRearJoint>rear_left_wheel</leftRearJoint>
<rightRearJoint>rear_right_wheel</rightRearJoint>
<wheelSeparation>4</wheelSeparation>
<wheelDiameter>0.32918</wheelDiameter>
<robotBaseFrame>base_link</robotBaseFrame>
<torque>1000</torque>
<commandTopic>cmd_vel</commandTopic>
<broadcastTF>true</broadcastTF>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<covariance_x>0.000100</covariance_x>
<covariance_y>0.000100</covariance_y>
<covariance_yaw>0.010000</covariance_yaw>
</plugin>
</gazebo> -->
<!-- <gazebo>
<plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<odometryRate>20.0</odometryRate>
<robotBaseFrame>base_footprint</robotBaseFrame>
</plugin>
</gazebo> -->
</robot>

View File

@@ -24,14 +24,30 @@
<xacro:property name="wheel_length" value="1.1653e-01" />
<xacro:property name="wheel_radius" value="1.6459e-01" />
<!-- Virtual link as root -->
<link name="vehicle_reference" />
<joint name="veh_ref_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="vehicle_reference" />
<child link="base_link" />
</joint>
<!-- Base link -->
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://scout_description/meshes/base_link_full.dae" />
<mesh filename="package://scout_description/meshes/base_link.dae" />
</geometry>
</visual>
<inertial>
<mass value="40" />
<!-- <mass value="50" /> -->
<!-- <origin xyz="-0.00065 -0.085 0.062" />
<inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296" /> -->
<origin xyz="0.0 0.0 0.0" />
<inertia ixx="2.288641" ixy="0" ixz="0" iyy="5.103976" iyz="0" izz="3.431465" />
</inertial>
<collision>
<origin xyz="0 0 0.008" rpy="0 0 0" />
<geometry>
@@ -55,21 +71,6 @@
<child link="base_footprint" />
</joint>
<!-- Interial link -->
<link name="inertial_link">
<inertial>
<mass value="46.034" />
<origin xyz="-0.00065 -0.085 0.062" />
<inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296" />
</inertial>
</link>
<joint name="inertial_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="base_link" />
<child link="inertial_link" />
</joint>
<!-- Scout wheel macros -->
<!-- wheel labeled from 0 to 3, conter-clockwise, starting from front right wheel -->
<!-- motor 1 and 2 (left side) are mechanically installed in a reversed direction -->

View File

@@ -28,23 +28,17 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="scout_wheel">
<xacro:macro name="scout_wheel_transmission" params="wheel_prefix">
<!-- kp spring constant, kd damping constant -->
<gazebo reference="${wheel_prefix}_wheel_link">
<mu1 value="1.0"/>
<mu2 value="0.6"/>
<mu2 value="0.9"/>
<kp value="10000000.0" />
<kd value="1.0" />
<fdir1 value="1 0 0"/>
<minDepth value="0.001" />
<maxContacts value="64"/>
</gazebo>
<!-- <gazebo reference="${wheel_prefix}_wheel_link">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0" />
<kd value="1.0" />
<fdir1 value="1 0 0"/>
</gazebo> -->
<transmission name="${wheel_prefix}_wheel_trans" type="SimpleTransmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="${wheel_prefix}_wheel_motor">

View File

@@ -30,9 +30,9 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
<xacro:macro name="scout_wheel_type1" params="wheel_prefix *joint_pose">
<link name="${wheel_prefix}_wheel_link">
<inertial>
<mass value="2.637" />
<mass value="3" />
<origin xyz="0 0 0" />
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.04411" iyz="0" izz="0.02467" />
<inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />

View File

@@ -30,9 +30,9 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
<xacro:macro name="scout_wheel_type2" params="wheel_prefix *joint_pose">
<link name="${wheel_prefix}_wheel_link">
<inertial>
<mass value="2.637" />
<mass value="3" />
<origin xyz="0 0 0" />
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.04411" iyz="0" izz="0.02467" />
<inertia ixx="0.7171" ixy="0" ixz="0" iyy="0.1361" iyz="0" izz="0.7171" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />

View File

@@ -1,36 +1,10 @@
<?xml version="1.0"?>
<!--
Software License Agreement (BSD)
\file husky_empty_world.launch
\authors Paul Bovbel <pbovbel@clearpathrobotics.com, Devon Ash <dash@clearpathrobotics.com>
\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that
the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this list of conditions and the
following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
following disclaimer in the documentation and/or other materials provided with the distribution.
* Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<launch>
<arg name="robot_namespace" default="/"/>
<arg name="world_name" default="worlds/empty.world"/>
<arg name="world_name" default="$(find scout_gazebo_sim)worlds/weston_robot_empty.world"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)"/>
<!-- world_name is wrt GAZEBO_RESOURCE_PATH environment variable -->
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>

View File

@@ -29,6 +29,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
<arg name="world_name" default="$(find scout_gazebo_sim)/worlds/clearpath_playpen.world"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="physics" value="bullet" />
<arg name="world_name" value="$(arg world_name)"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>

View File

@@ -0,0 +1,29 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<!-- Physics -->
<!-- <physics type="ode">
<ode>
<solver>
<type>world</type>
</solver>
<constraints>
<contact_max_correcting_vel>0.1</contact_max_correcting_vel>
<contact_surface_layer>0.0001</contact_surface_layer>
</constraints>
</ode>
<max_step_size>0.001</max_step_size>
</physics>
<gravity>0.0 0.0 -9.81</gravity> -->
</world>
</sdf>