Skip to content
Snippets Groups Projects
Verified Commit 8f947e1e authored by Justin Carpentier's avatar Justin Carpentier
Browse files

models: correct new display function

parent 9e5edf71
No related branches found
No related tags found
No related merge requests found
...@@ -4,7 +4,7 @@ ...@@ -4,7 +4,7 @@
import pinocchio as pin import pinocchio as pin
from math import pi from math import pi
from pinocchio.utils import np, rotate, XYZQUATToViewerConfiguration, se3ToXYZQUAT from pinocchio.utils import np, rotate
DENSITY = 1 DENSITY = 1
...@@ -105,7 +105,7 @@ class RobotDisplay(object): ...@@ -105,7 +105,7 @@ class RobotDisplay(object):
self.viewer.gui.refresh() self.viewer.gui.refresh()
def place(self, obj_name, m): def place(self, obj_name, m):
self.viewer.gui.applyConfiguration(obj_name, XYZQUATToViewerConfiguration(se3ToXYZQUAT(m))) self.viewer.gui.applyConfiguration(obj_name,pin.se3ToXYZQUATtuple(m))
class SimplestWalker(ModelWrapper): class SimplestWalker(ModelWrapper):
......
...@@ -3,11 +3,10 @@ from math import sqrt ...@@ -3,11 +3,10 @@ from math import sqrt
import numpy as np import numpy as np
import pinocchio as pin import pinocchio as pin
from pinocchio.utils import (XYZQUATToSe3, cross, isapprox, se3ToXYZQUAT) from pinocchio.utils import cross, isapprox
from test_case import TestCase from test_case import TestCase
class TestUtils(TestCase): class TestUtils(TestCase):
def test_cross(self): def test_cross(self):
a = np.matrix('2. 0. 0.').T a = np.matrix('2. 0. 0.').T
...@@ -19,8 +18,8 @@ class TestUtils(TestCase): ...@@ -19,8 +18,8 @@ class TestUtils(TestCase):
m = pin.SE3.Identity() m = pin.SE3.Identity()
m.translation = np.matrix('1. 2. 3.').T m.translation = np.matrix('1. 2. 3.').T
m.rotation = np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.') # rotate('x', pi / 2) m.rotation = np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.') # rotate('x', pi / 2)
self.assertApprox(se3ToXYZQUAT(m).T, [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]) self.assertApprox(pin.se3ToXYZQUAT(m).T, [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2])
self.assertApprox(XYZQUATToSe3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m) self.assertApprox(pin.XYZQUATToSe3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m)
def test_isapprox(self): def test_isapprox(self):
self.assertFalse(isapprox(1, 2)) self.assertFalse(isapprox(1, 2))
......
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