Skip to content

Instantly share code, notes, and snippets.

@rpapallas

rpapallas/save_env_to_file.py

Last active April 24, 2018 09:52
Show Gist options
  • Save rpapallas/4031a02704a4fd95f0c3004f0c56c48e to your computer and use it in GitHub Desktop.
Save rpapallas/4031a02704a4fd95f0c3004f0c56c48e to your computer and use it in GitHub Desktop.
Download ZIP
Export OpenRAVE Environment to XML File
Raw
save_env_to_file.py
"""
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
"""
import time
import datetime
def get_translation_from_transform(transform):
translation = ""
for row in range(0, 3):
translation += str(transform[row][3])
if row != 2: translation += " "
return translation
def get_rotation_matrix_from_transform(transform):
rotation = ""
for row in range(0, 3):
for col in range(0, 3):
rotation += str(transform[row, col])
if row != 2 or col != 2: rotation += " "
return rotation
def save_current_env_configuration_to_xml(exclude=["ur5", "baxter", "ProjectRoom"], template_file_path="environments/template.xml", output_path="environments/"):
with open(template_file_path, "r") as input_file:
data = input_file.read()
for kinbody in env.GetBodies():
kinbody_name = kinbody.GetName()
if kinbody_name not in exclude:
transform = kinbody.GetTransform()
translation = get_translation_from_transform(transform)
rotation_matrix = get_rotation_matrix_from_transform(transform)
data = data.replace("{" + kinbody_name + "_t}", translation)
data = data.replace("{" + kinbody_name + "_rm}", rotation_matrix)
ts = time.time()
timestamp = datetime.datetime.fromtimestamp(ts).strftime("%Y-%m-%d %H:%M:%S")
with open("{}{}.xml".format(output_path, timestamp), "w") as output_file:
output_file.write(data)
Raw
template.xml
<!--
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
A template xml file with all the kinbodies etc. Just make sure that the <translation></translation>
and <rotationmat></rotationnat> tags are there for the objects you wish to export. In between those tags
add {kinbody_name_t} and {kinbody_name_rm} respectively. For example, if you have a kinbody called 'can_1'
then you need:
<Translation>{can_1_t}</Translation>
<RotationMat>{can_1_rm}</RotationMat>
The Python script will find this convention and will replace it with the current translation and rotation matrix
of the kinbody.
-->
<Environment>
<KinBody name="can_1">
<Translation>{can_1_t}</Translation>
<RotationMat>{can_1_rm}</RotationMat>
<Body name="Base" type="dynamic">
<Geom type="cylinder">
<rotationaxis>1 0 0 90</rotationaxis>
<translation>0.0 0.0 0.058</translation>
<radius>0.03305</radius>
<height>0.1161</height>
<diffuseColor>0.16 0.46 0.03</diffuseColor>
</Geom>
</Body>
</KinBody>
</Environment>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment