Skip to content
Snippets Groups Projects
Commit ea651598 authored by Lauritz Keysberg's avatar Lauritz Keysberg
Browse files

Changed inertia and physical wheelsize to be indepently changeable. Added...

Changed inertia and physical wheelsize to be indepently changeable. Added python urdf creator for amiro.
parent 194f61e4
Branches master
No related tags found
No related merge requests found
......@@ -10,6 +10,12 @@
<xacro:property name="base_radius" value="${base_diameter / 2}" /> <!-- (m) base radius -->
<xacro:property name="base_height" value="0.084" /> <!-- (m) base height from bottom of DiWheelDrive + extension + lid -->
<xacro:property name="base_clearance" value="0.01" /> <!-- (m) space between floor and base -->
<xacro:property name="wheel_height" value="0.006" /> <!-- (m) wheel height -->
<xacro:property name="wheel_sep" value="0.069" /> <!-- (m) wheel seperation -->
<xacro:property name="wheel_dia" value="0.05671" /> <!-- (m) wheel diameter -->
<xacro:property name="wheel_radius" value="${lwheel_dia / 2}" /> <!-- (m) wheel radius -->
<xacro:property name="wheel_mass" value="0.02" /> <!-- (kg) wheel mass -->
<xacro:property name="wheel_damping" value="0.1" /> <!-- (?) wheel damping -->
<xacro:property name="lwheel_height" value="0.006" /> <!-- (m) wheel height -->
<xacro:property name="lwheel_sep" value="0.069" /> <!-- (m) wheel seperation -->
<xacro:property name="lwheel_dia" value="0.05571" /> <!-- (m) wheel diameter -->
......@@ -88,12 +94,12 @@
<xacro:property name="base_ixx" value="${1/12 * base_mass * (3*base_radius*base_radius+base_height*base_height)}" />
<xacro:property name="base_iyy" value="${base_ixx}" />
<xacro:property name="base_izz" value="${1/2*base_mass*base_radius*base_radius}" />
<xacro:property name="lwheel_ixx" value="${1/12 * lwheel_mass * (3*lwheel_radius*lwheel_radius+lwheel_height*lwheel_height)}" />
<xacro:property name="lwheel_ixx" value="${1/12 * wheel_mass * (3*wheel_radius*wheel_radius+wheel_height*wheel_height)}" />
<xacro:property name="lwheel_iyy" value="${lwheel_ixx}" />
<xacro:property name="lwheel_izz" value="${1/2*lwheel_mass*lwheel_radius*lwheel_radius}" />
<xacro:property name="rwheel_ixx" value="${1/12 * rwheel_mass * (3*rwheel_radius*rwheel_radius+rwheel_height*rwheel_height)}" />
<xacro:property name="lwheel_izz" value="${1/2*wheel_mass*wheel_radius*wheel_radius}" />
<xacro:property name="rwheel_ixx" value="${1/12 * wheel_mass * (3*wheel_radius*wheel_radius+wheel_height*wheel_height)}" />
<xacro:property name="rwheel_iyy" value="${rwheel_ixx}" />
<xacro:property name="rwheel_izz" value="${1/2*rwheel_mass*rwheel_radius*rwheel_radius}" />
<xacro:property name="rwheel_izz" value="${1/2*wheel_mass*wheel_radius*wheel_radius}" />
<xacro:property name="caster_i" value="${2/5*caster_mass*caster_radius*caster_radius}" />
......
......@@ -10,7 +10,7 @@
<node pkg="display_rs" name="robot_display_node" type="robot_display_node.py" output="screen">
<param name="coordinate_list" value="$(find display_rs)/maps/showcase_coordinates_angles.txt" />
<param name="x_pos" value="0.337732" />
<param name="x_pos" value="0.137732" />
<param name="y_pos" value="0.557183" />
</node>
......
# Emacs Files
.#*
# Allow user to enable or disable this w/o changing repo
amiro_gazebo_plugins/CATKIN_IGNORE
\ No newline at end of file
[submodule "amiro_msgs"]
path = amiro_msgs
url = https://github.com/autonomoussystemsengineering/amiro_msgs.git
#!/usr/bin/python
from odio_urdf import *
import math
"""
ROS urdf definition of the AMiRo
Author: Lauritz Keysberg
Inertial matrix has to be in CoM frame (URDF specs)
Velocity and effort limits are fake!
"""
model_name = "AMiRo odio_urdf"
model_version = "0.0"
# This description yet always assumes gazebo and mesh model as true
USE_GAZEBO = True
joint_types = "continuous"
USE_MESH_MODEL = True
### Constants for robot dimensions ###
PI = 3.141592653589793
PI_2 = PI / 2
deg2rad = PI / 180
base_mass = 0.4# (kg) base mass
base_diameter = 0.099 # (m) base diameter
base_radius = base_diameter / 2 # (m) base radius
base_height = 0.084 # (m) base height from bottom of DiWheelDrive + extension + lid
base_clearance = 0.01 # (m) space between floor and base
lwheel_height = 0.006 # (m) wheel height
lwheel_sep = 0.069 # (m) wheel seperation
lwheel_dia = 0.05571 # (m) wheel diameter
lwheel_radius = lwheel_dia / 2 # (m) wheel radius
lwheel_mass = 0.02 # (kg) wheel mass
lwheel_damping = 0.1 # (?) wheel damping
rwheel_height = 0.006 # (m) wheel height
rwheel_sep = 0.069 # (m) wheel seperation
rwheel_dia = 0.05571 # (m) wheel diameter
rwheel_radius = rwheel_dia / 2 # (m) wheel radius
rwheel_mass = 0.02 # (kg) wheel mass
rwheel_damping = 0.1 # (?) wheel damping
wheel_sep = lwheel_sep / 2 + rwheel_sep / 2 # (m) total wheel seperation
wheel_dia = 0.05571 # (m) assumed wheel diameter
caster_mass = 0.01 # (kg) caster mass
caster_dia = 0.005 # (m) caster diameter
caster_radius = caster_dia / 2 # (m) caster radius
caster_clearance = 0.001 # (m) caster clearance above ground (needed, so that the robot is movable)
caster_sep = 0.072 # (m) caster seperation (we assume that there are two symetric caster in the front and back)
### Mesh model parameters ###
mesh_scale = 0.0254
mesh_clearance = base_clearance + 0.002
### Constant dependent parameters ###
base_link_height = base_clearance + base_height / 2 # base link height -->
caster_link_height = caster_clearance + caster_radius # caster link height -->
lid_link_height = base_clearance + base_height # top of lid from base_link -->
camera_link_height = base_clearance + 0.032 # camera height from base_link -->
camera_link_depth = base_radius # camera height from base_link -->
proximity_floor_z = base_clearance # Proximity floor sensor over ground -->
proximity_floor_x_0 = base_radius - 0.005
proximity_floor_y_0 = -0.0035
proximity_floor_x_1 = base_radius - 0.021
proximity_floor_y_1 = -0.032
proximity_floor_x_2 = base_radius - 0.021
proximity_floor_y_2 = 0.032
proximity_floor_x_3 = base_radius - 0.005
proximity_floor_y_3 = 0.0035
proximity_ring_z = 0.027 + base_clearance # Proximity ring sensor over ground -->
proximity_ring_yaw_3 = 22.5 * deg2rad
proximity_ring_yaw_2 = 67.5 * deg2rad
proximity_ring_yaw_1 = 112.5 * deg2rad
proximity_ring_yaw_0 = 157.5 * deg2rad
proximity_ring_yaw_7 = 202.5 * deg2rad
proximity_ring_yaw_6 = 247.5 * deg2rad
proximity_ring_yaw_5 = 292.5 * deg2rad
proximity_ring_yaw_4 = 337.5 * deg2rad
proximity_ring_x_0 = base_radius * math.cos(proximity_ring_yaw_0)
proximity_ring_y_0 = base_radius * math.sin(proximity_ring_yaw_0)
proximity_ring_x_1 = base_radius * math.cos(proximity_ring_yaw_1)
proximity_ring_y_1 = base_radius * math.sin(proximity_ring_yaw_1)
proximity_ring_x_2 = base_radius * math.cos(proximity_ring_yaw_2)
proximity_ring_y_2 = base_radius * math.sin(proximity_ring_yaw_2)
proximity_ring_x_3 = base_radius * math.cos(proximity_ring_yaw_3)
proximity_ring_y_3 = base_radius * math.sin(proximity_ring_yaw_3)
proximity_ring_x_4 = base_radius * math.cos(proximity_ring_yaw_4)
proximity_ring_y_4 = base_radius * math.sin(proximity_ring_yaw_4)
proximity_ring_x_5 = base_radius * math.cos(proximity_ring_yaw_5)
proximity_ring_y_5 = base_radius * math.sin(proximity_ring_yaw_5)
proximity_ring_x_6 = base_radius * math.cos(proximity_ring_yaw_6)
proximity_ring_y_6 = base_radius * math.sin(proximity_ring_yaw_6)
proximity_ring_x_7 = base_radius * math.cos(proximity_ring_yaw_7)
proximity_ring_y_7 = base_radius * math.sin(proximity_ring_yaw_7)
### http://en.wikipedia.org/wiki/List_of_moment_of_inertia_tensors ###
base_ixx = 1/12 * base_mass * (3*base_radius*base_radius+base_height*base_height)
base_iyy = base_ixx
base_izz = 1/2*base_mass*base_radius*base_radius
lwheel_ixx = 1/12 * lwheel_mass * (3*lwheel_radius*lwheel_radius+lwheel_height*lwheel_height)
lwheel_iyy = lwheel_ixx
lwheel_izz = 1/2*lwheel_mass*lwheel_radius*lwheel_radius
rwheel_ixx = 1/12 * rwheel_mass * (3*rwheel_radius*rwheel_radius+rwheel_height*rwheel_height)
rwheel_iyy = rwheel_ixx
rwheel_izz = 1/2*rwheel_mass*rwheel_radius*rwheel_radius
caster_i = 2/5*caster_mass*caster_radius*caster_radius
### Topic names ###
odom = "odom" # odometry topic/tf
cmd_vel = "cmd_vel" # differential command topic
########### Robot definition ###############
amiro = Robot("amiro")
amiro(
Link(
Collision(
Origin(xyz=f"0 0 {base_link_height}", rpy="0 0 0"),
Geometry(
Cylinder(length=f"{base_height}", radius=f"{base_radius}")
),
name="base_collision"
),
Visual(
Origin(xyz=f"{base_radius} {-base_radius} {mesh_clearance}", rpy=f"0 0 {PI/2}"),
Geometry(
Mesh(filename="package://amiro_description/meshes/robot/ExtendedAmiroOnly_degenerateDissolve_cleanUpGeometry005.dae", scale=f"{mesh_scale} {mesh_scale} {mesh_scale}")
)
),
name="base_link"
),
Joint(
Parent(link="base_link"),
Child(link="base_inertia_link"),
Origin(xyz="0 0 0", rpy="0 0 0"),
Axis(xyz="1 1 1"),
Dynamics(damping="0.0"),
name="base_inertia_joint",
type="fixed"
),
Link(
Inertial(
Origin(xyz=f"0 0 {base_link_height}", rpy="0 0 0"),
Mass(value=f"{base_mass}"),
Inertia(
ixx=f"{base_ixx}",
ixy=f"{0.0}",
ixz=f"{0.0}",
iyy=f"{base_iyy}",
iyz=f"{0.0}",
izz=f"{base_izz}"
)
),
name="base_inertia_link"
),
Joint(
Parent(link="base_link"),
Child(link="front_caster_link"),
Origin(xyz=f"{caster_sep / 2} 0 {caster_link_height}", rpy="0 0 0"),
Axis(xyz="1 1 1"),
Dynamics(damping="0.0"),
name="front_caster_joint",
type="fixed"
),
Link(
Collision(
Origin(xyz="0 0 0", rpy="0 0 0"),
Geometry(
Sphere(radius=f"{caster_radius}")
),
name="caster_front_collision"
),
Inertial(
Origin(xyz="0 0 0", rpy="0 0 0"),
Mass(value=f"{caster_mass}"),
Inertia(
ixx=f"{caster_i}",
ixy=f"{0.0}",
ixz=f"{0.0}",
iyy=f"{caster_i}",
iyz=f"{0.0}",
izz=f"{caster_i}"
)
),
name="front_caster_link"
),
Joint(
Parent(link="base_link"),
Child(link="back_caster_link"),
Origin(xyz=f"{-caster_sep / 2} 0 {caster_link_height}", rpy="0 0 0"),
Axis(xyz="1 1 1"),
Dynamics(damping="0.0"),
name="back_caster_joint",
type="fixed"
),
Link(
Collision(
Origin(xyz="0 0 0", rpy="0 0 0"),
Geometry(
Sphere(radius=f"{caster_radius}")
),
name="caster_back_collision"
),
Inertial(
Origin(xyz="0 0 0", rpy="0 0 0"),
Mass(value=f"{caster_mass}"),
Inertia(
ixx=f"{caster_i}",
ixy=f"{0.0}",
ixz=f"{0.0}",
iyy=f"{caster_i}",
iyz=f"{0.0}",
izz=f"{caster_i}"
)
),
name="back_caster_link"
),
Joint(
Parent(link="base_link"),
Child(link="left_wheel_link"),
Origin(xyz=f"0 {lwheel_sep /2} {lwheel_radius}", rpy=f"0 {PI/2} {PI/2}"),
Axis(xyz="0 0 1"),
Dynamics(damping=f"{lwheel_damping}"),
name="left_wheel_joint",
type=f"{joint_types}"
),
Link(
Collision(
Cylinder(length=f"{lwheel_height}", radius=f"{lwheel_radius}"),
Geometry(
Cylinder(length=f"{lwheel_height}", radius=f"{lwheel_radius}")
)
),
Visual(
Origin(xyz="0 0 0", rpy=f"{PI/2} 0 0"),
Geometry(
Mesh(filename="package://amiro_description/meshes/robot/WheelDrive_degenerateDissolve_cleanUpGeometry002_centered.dae", scale=f"{mesh_scale} {mesh_scale} {mesh_scale}")
)
),
Inertial(
Cylinder(length=f"{lwheel_height}", radius=f"{lwheel_radius}"),
Mass(value=f"{lwheel_mass}"),
Inertia(
ixx=f"{lwheel_ixx}",
ixy=f"{0.0}",
ixz=f"{0.0}",
iyy=f"{lwheel_iyy}",
iyz=f"{0.0}",
izz=f"{lwheel_izz}"
)
),
name="left_wheel_link"
),
Joint(
Parent(link="base_link"),
Child(link="right_wheel_link"),
Origin(xyz=f"0 {-rwheel_sep /2} {rwheel_radius}", rpy=f"0 {PI/2} {PI/2}"),
Axis(xyz="0 0 1"),
Dynamics(damping=f"{rwheel_damping}"),
name="right_wheel_joint",
type=f"{joint_types}"
),
Link(
Collision(
Cylinder(length=f"{rwheel_height}", radius=f"{rwheel_radius}"),
Geometry(
Cylinder(length=f"{rwheel_height}", radius=f"{rwheel_radius}")
)
),
Visual(
Origin(xyz="0 0 0", rpy=f"{PI/2} 0 0"),
Geometry(
Mesh(filename="package://amiro_description/meshes/robot/WheelDrive_degenerateDissolve_cleanUpGeometry002_centered.dae", scale=f"{mesh_scale} {mesh_scale} {mesh_scale}")
)
),
Inertial(
Cylinder(length=f"{rwheel_height}", radius=f"{rwheel_radius}"),
Mass(value=f"{rwheel_mass}"),
Inertia(
ixx=f"{rwheel_ixx}",
ixy=f"{0.0}",
ixz=f"{0.0}",
iyy=f"{rwheel_iyy}",
iyz=f"{0.0}",
izz=f"{rwheel_izz}"
)
),
name="right_wheel_link"
)
)
print(amiro)
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment