And its time for a restart!
I was following this the other day:
http://gazebosim.org/tutorials?cat=guid ... =guided_i2
Look they are using FreeCAD!
There are only two issues:
- Gazebo/SDF wants the models to have its model origo at the center of mass.
- Gazebo/SDF uses meters, not millimeters.. so make a model 5mm and BOOM! you got a 5m part in gazebo.
In the guide above they center and scale the object by exporting a mesh from FreeCAD and load it in Blender.. WTF? so that had me going.
Now i have hacked together a "export script":
Code: Select all
print "Scaling mesh"
import Mesh,BuildRegularGeoms
mat=FreeCAD.Matrix()
mat.scale(0.001,0.001,0.001)
obj = App.ActiveDocument.Objects[0]
obj.Name
mesh=App.ActiveDocument.Objects[0].Mesh.copy()
mesh.transform(mat)
Mesh.show(mesh)
__objs__=[]
__objs__.append(FreeCAD.ActiveDocument.getObject("Mesh"))
Mesh.export(__objs__,u"/home/maiden/Projects/tinyArm/"+obj.Name+".stl")
print "Saved mesh to /home/maiden/Projects/tinyArm/"+obj.Name+".stl"
App.ActiveDocument.removeObject("Mesh")
And something for aligning the model according to center of mass:
Code: Select all
com = FreeCAD.getDocument("Unnamed1").getObject("Body").Shape.CenterOfMass
FreeCAD.getDocument("Unnamed1").getObject("Body").Placement = FreeCAD.getDocument("Unnamed1").getObject("Body").Placement.move(com*-1)
Now, this is just hacks/proof of concept. need to make them a bit less hard coded and more dynamic.
Things i want to add from here:
1. Generate a "dummy SDF" with data from a FreeCAD document with one shape (pose, inertia, mesh)
2. Generate a "dummy SDF" with data from a FreeCAD document with several shapes
3. Create a visual "dummy object" in FreeCAD that links ShapeA and ShapeB at object location and exports as a joint to a SDF file
4. Create a python script to load the model in gazebo and controlling the joints from FreeCAD.
5. Build workbench around this.
Then it will be time to play around with some rapid robotic prototyping
Example SDF:
Code: Select all
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="baseBot">
<!-- Give the base link a unique name -->
<link name="base">
<!-- Offset the base by half the lenght of the cylinder -->
<pose>0 0 -0.005 0 0 0</pose>
<inertial>
<mass>1.2</mass>
<inertia>
<ixx>0.0010608477766732891</ixx>
<iyy>-0.00023721360899498273</iyy>
<izz>0.00024999999996488216</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name="base_collision">
<geometry>
<mesh>
<!-- The URI should refer to the 3D mesh. The "model:"
URI scheme indicates that the we are referencing a Gazebo
model. -->
<uri>model://baseBot/meshes/plate.stl</uri>
</mesh>
</geometry>
</collision>
<!-- The visual is mostly a copy of the collision -->
<visual name="base_visual">
<geometry>
<mesh>
<!-- The URI should refer to the 3D mesh. The "model:"
URI scheme indicates that the we are referencing a Gazebo
model. -->
<uri>model://baseBot/meshes/plate.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<!-- Give the base link a unique name -->
<link name="top">
<!-- Vertically offset the top cylinder by the length of the bottom
cylinder and half the length of this cylinder. -->
<pose>0.00 0.00 0.005 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.0010608477766732891</ixx>
<iyy>-0.00023721360899498273</iyy>
<izz>0.00024999999996488216</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name="top_collision">
<geometry>
<mesh>
<uri>model://baseBot/meshes/holder.stl</uri>
</mesh>
</geometry>
</collision>
<!-- The visual is mostly a copy of the collision -->
<visual name="top_visual">
<geometry>
<mesh>
<uri>model://baseBot/meshes/holder.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<!-- Each joint must have a unique name -->
<joint type="revolute" name="joint">
<!-- Position the joint at the bottom of the top link -->
<pose>0.00 0.00 -0.00048 0 0 0</pose>
<!-- Use the base link as the parent of the joint -->
<parent>base</parent>
<!-- Use the top link as the child of the joint -->
<child>top</child>
<!-- The axis defines the joint's degree of freedom -->
<axis>
<!-- Revolve around the z-axis -->
<xyz>0 0 1</xyz>
<!-- Limit refers to the range of motion of the joint -->
<limit>
<!-- Use a very large number to indicate a continuous revolution -->
<lower>-10000000000000000</lower>
<upper>10000000000000000</upper>
</limit>
</axis>
</joint>
</model>
</sdf>