Unverified Commit 73409534 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #8 from pFernbach/devel

Add script to load examples in gepetto-gui
parents 72aa47cd 31cc74a1
Pipeline #9485 passed with stage
in 6 minutes and 16 seconds
......@@ -24,6 +24,20 @@ cs = ContactSequence()
cs.loadFromBinary(filename)
```
## Display the motion in gepetto-gui
A script is provided to load a motion and display it in gepetto-gui, this script require pinocchio (with python bindings) and gepetto-gui.
```
python3 display_gepetto_gui.py CS_WB_NAME
```
Optionally, you can specify an environment to load in the viewer (the default is a flat floor at z=0)
```
python3 display_gepetto_gui.py CS_WB_NAME --env_name multicontact/plateforme_surfaces
```
## Suffix notation
For the same scenario, several files may exist with different Suffixes, here is the meaning of this Suffixes:
......
import curves
from multicontact_api import ContactSequence
import gepetto.corbaserver
import pinocchio as pin
from rospkg import RosPack
import time
import argparse
import subprocess
import atexit
import os
# Define robot model
robot_package_name = "talos_data"
urdf_name = "talos_reduced"
# Define environment
env_package_name = "hpp_environments"
env_name = "multicontact/ground" # default value, may be defined with argument
scene_name = "world"
# timestep used to display the configurations
DT_DISPLAY = 0.04 # 25 fps
def display_wb(robot, q_t):
t = q_t.min()
while t <= q_t.max():
t_start = time.time()
robot.display(q_t(t))
t += DT_DISPLAY
elapsed = time.time() - t_start
if elapsed > DT_DISPLAY:
print("Warning : display not real time ! choose a greater time step for the display.")
else:
time.sleep(DT_DISPLAY - elapsed)
# display last config if the total duration is not a multiple of the dt
robot.display(q_t(q_t.max()))
if __name__ == '__main__':
# Get cs_name from the arguments:
parser = argparse.ArgumentParser(description="Load a ContactSequence and display the joint-trajectory in gepetto-gui")
parser.add_argument('cs_name', type=str, help="The name of the serialized ContactSequence file")
parser.add_argument('--env_name', type=str, help="The name of environment urdf file in hpp_environments")
args = parser.parse_args()
cs_name = args.cs_name
if args.env_name:
env_name = args.env_name
# Start the gepetto-gui background process
subprocess.run(["killall", "gepetto-gui"])
process_viewer = subprocess.Popen("gepetto-gui",
stdout=subprocess.PIPE,
stderr=subprocess.DEVNULL,
preexec_fn=os.setpgrp)
atexit.register(process_viewer.kill)
# Load robot model in pinocchio
rp = RosPack()
urdf = rp.get_path(robot_package_name) + '/urdf/' + urdf_name + '.urdf'
robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer())
robot.initDisplay(loadModel=True)
robot.displayCollisions(False)
robot.displayVisuals(True)
# Load environment model
cl = gepetto.corbaserver.Client()
gui = cl.gui
env_package_path = rp.get_path(env_package_name)
env_urdf_path = env_package_path + '/urdf/' + env_name + '.urdf'
gui.addUrdfObjects(scene_name + "/environments", env_urdf_path, True)
# Load the motion from the multicontact-api file
cs = ContactSequence()
cs.loadFromBinary(cs_name)
assert cs.haveJointsTrajectories(), "The file loaded do not have joint trajectories stored."
q_t = cs.concatenateQtrajectories()
display_wb(robot, q_t)
No preview for this file type
No preview for this file type
......@@ -143,14 +143,14 @@ BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_WB) {
BOOST_AUTO_TEST_CASE(walk_20cm) {
ContactSequence cs;
cs.loadFromBinary(path + "walk_20cm.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK_EQUAL(cs.size(), 15);
BOOST_CHECK(cs.haveConsistentContacts());
}
BOOST_AUTO_TEST_CASE(walk_20cm_COM) {
ContactSequence cs;
cs.loadFromBinary(path + "walk_20cm_COM.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK_EQUAL(cs.size(), 15);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
BOOST_CHECK(cs.haveCentroidalValues());
......@@ -160,18 +160,18 @@ BOOST_AUTO_TEST_CASE(walk_20cm_COM) {
BOOST_AUTO_TEST_CASE(walk_20cm_REF) {
ContactSequence cs;
cs.loadFromBinary(path + "walk_20cm_REF.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK_EQUAL(cs.size(), 15);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
BOOST_CHECK(cs.haveCentroidalValues());
BOOST_CHECK(cs.haveCentroidalTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories());
BOOST_CHECK(cs.haveEffectorsTrajectories(1e-2));
}
BOOST_AUTO_TEST_CASE(walk_20cm_WB) {
ContactSequence cs;
cs.loadFromBinary(path + "walk_20cm_WB.cs");
BOOST_CHECK_EQUAL(cs.size(), 23);
BOOST_CHECK_EQUAL(cs.size(), 15);
BOOST_CHECK(cs.haveConsistentContacts());
BOOST_CHECK(cs.haveTimings());
BOOST_CHECK(cs.haveCentroidalValues());
......
......@@ -211,13 +211,13 @@ class ExamplesSerialization(unittest.TestCase):
def test_walk_20cm(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm.cs"))
self.assertEqual(cs.size(), 23)
self.assertEqual(cs.size(), 15)
self.assertTrue(cs.haveConsistentContacts())
def test_walk_20cm_COM(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_COM.cs"))
self.assertEqual(cs.size(), 23)
self.assertEqual(cs.size(), 15)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
......@@ -227,18 +227,18 @@ class ExamplesSerialization(unittest.TestCase):
def test_walk_20cm_REF(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_REF.cs"))
self.assertEqual(cs.size(), 23)
self.assertEqual(cs.size(), 15)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
self.assertTrue(cs.haveCentroidalTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories())
self.assertTrue(cs.haveEffectorsTrajectories(1e-2))
checkCS(self, cs, root=True, effector=True, wholeBody=False)
def test_walk_20cm_WB(self):
cs = ContactSequence()
cs.loadFromBinary(str(PATH / "walk_20cm_WB.cs"))
self.assertEqual(cs.size(), 23)
self.assertEqual(cs.size(), 15)
self.assertTrue(cs.haveConsistentContacts())
self.assertTrue(cs.haveTimings())
self.assertTrue(cs.haveCentroidalValues())
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment