Skip to content

Commit

Permalink
python: fix other np.matrix issues
Browse files Browse the repository at this point in the history
  • Loading branch information
jcarpent committed Apr 28, 2023
1 parent 6a0b9c8 commit 08d6c5e
Show file tree
Hide file tree
Showing 16 changed files with 54 additions and 56 deletions.
6 changes: 3 additions & 3 deletions bindings/python/pinocchio/derivative/dcrba.py
Original file line number Diff line number Diff line change
Expand Up @@ -83,8 +83,8 @@ def __call__(self):

for d in iv(joint_diff):

T_iSd = np.matrix(H[:,d,i0:i1]) # this is 0 if d<=i (<=j)
T_jSd = np.matrix(H[:,d,j0:j1]) # this is 0 is d<=j
T_iSd = np.array(H[:,d,i0:i1]) # this is 0 if d<=i (<=j)
T_jSd = np.array(H[:,d,j0:j1]) # this is 0 is d<=j

'''
assert( norm(T_iSd)<1e-6 or not joint_diff<i ) # d<i => TiSd=0
Expand Down Expand Up @@ -254,7 +254,7 @@ def __call__(self,q,vq):
# CEN ===> Sii' vi x Yi Si = (Sii x vi)' Yi Si
C[ii0:ii1,i0:i1] += Sii.T*vxYS
# CEN ===> Sii' Si x Yi vi = (Sii x Si)' Yi vi
C[ii0:ii1,i0:i1] += np.matrix(td(H[:,i0:i1,ii0:ii1],Yv,[0,0])[:,:,0]).T
C[ii0:ii1,i0:i1] += np.array(td(H[:,i0:i1,ii0:ii1],Yv,[0,0])[:,:,0])

return C

Expand Down
2 changes: 1 addition & 1 deletion bindings/python/pinocchio/derivative/lambdas.py
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ def setRobotArgs(robot):
adjdual.__doc__ = "Force pre-cross product adjdual(a) = -adj(a)' "

td = np.tensordot
quad = lambda H,v: np.matrix(td(td(H,v,[2,0]),v,[1,0])).T
quad = lambda H,v: np.array(td(td(H,v,[2,0]),v,[1,0]))
quad.__doc__ = '''Tensor product v'*H*v, with H.shape = [ nop, nv, nv ]'''

def np_prettyprint(sarg = '{: 0.5f}',eps=5e-7):
Expand Down
2 changes: 0 additions & 2 deletions bindings/python/pinocchio/romeo_wrapper.py
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,6 @@ def __init__(self, filename, package_dirs=None, verbose=False):
0, 0, 0, 0, # head
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # right arm
])
if pin.getNumpyType()==np.matrix:
self.q0 = np.matrix(self.q0).T

self.opCorrespondances = {"lh": "LWristPitch",
"rh": "RWristPitch",
Expand Down
2 changes: 1 addition & 1 deletion doc/d-practical-exercises/1-directgeom.md
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ the following example.

```py
import numpy as np
A = np.matrix([[1, 2, 3, 4], [5, 6, 7, 8]]) # Define a 2x4 matrix
A = np.array([[1, 2, 3, 4], [5, 6, 7, 8]]) # Define a 2x4 matrix
b = np.zeros([4, 1]) # Define a 4 vector (ie a 4x1 matrix) initialized with 0
c = A * b # Obtain c by multiplying A by b.
```
Expand Down
2 changes: 1 addition & 1 deletion doc/d-practical-exercises/2-invgeom.md
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ from a SciPy-like vector to a Pinocchio-like vector using:
```py
import numpy as np
x = np.array([1.0, 2.0, 3.0])
q = np.matrix(x).T
q = np.array(x)
x = q.getA()[:, 0]
```

Expand Down
2 changes: 1 addition & 1 deletion doc/d-practical-exercises/3-invkine.md
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ def Rquat(x, y, z, w):
q.normalize()
return q.matrix()

Mgoal = pin.SE3(Rquat(.4, .02, -.5, .7), np.matrix([.2, -.4, .7]).T)
Mgoal = pin.SE3(Rquat(.4, .02, -.5, .7), np.array([.2, -.4, .7]))
robot.viewer.gui.addXYZaxis('world/framegoal', [1., 0., 0., 1.], .015, 4)
place('world/framegoal', Mgoal)
```
Expand Down
2 changes: 1 addition & 1 deletion doc/d-practical-exercises/5-planner.md
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ below:
obs = se3.GeometryObject.CreateCapsule(rad, length) # Pinocchio obstacle object
obs.name = "obs" # Set object name
obs.parentJoint = 0 # Set object parent = 0 = universe
obs.placement = se3.SE3(rotate('x', .1) * rotate('y', .1) * rotate('z', .1), np.matrix([.1, .1, .1]).T) # Set object placement wrt parent
obs.placement = se3.SE3(rotate('x', .1) * rotate('y', .1) * rotate('z', .1), np.array([.1, .1, .1])) # Set object placement wrt parent
robot.collision_model.addGeometryObject(obs, robot.model, False) # Add object to collision model
robot.visual_model.addGeometryObject(obs, robot.model, False) # Add object to visual model
# Also create a geometric object in gepetto viewer, with according name.
Expand Down
4 changes: 2 additions & 2 deletions doc/d-practical-exercises/6-wpg.md
Original file line number Diff line number Diff line change
Expand Up @@ -27,10 +27,10 @@ verbose output of BFGS. For example, the robot can track a target moving
vertically using the following example:

```py
cost.Mdes = se3.SE3(eye(3), np.matrix([0.2, 0, 0.1 + t / 100.])) # Reference target at time 0.
cost.Mdes = se3.SE3(eye(3), np.array([0.2, 0, 0.1 + t / 100.])) # Reference target at time 0.
q = np.copy(robot.q0)
for t in range(100):
cost.Mdes.translation = np.matrix([0.2, 0, 0.1 + t / 100.])
cost.Mdes.translation = np.array([0.2, 0, 0.1 + t / 100.])
q = fmin_bfgs(cost, q, maxiter=10, disp=False)
robot.display(q)
```
Expand Down
4 changes: 2 additions & 2 deletions doc/d-practical-exercises/src/dpendulum.py
Original file line number Diff line number Diff line change
Expand Up @@ -85,11 +85,11 @@ def step(self,iu):

def render(self):
q = d2cq(self.x[0])
self.pendulum.display(np.matrix([q,]))
self.pendulum.display(np.array([q,]))
time.sleep(self.pendulum.DT)

def dynamics(self,ix,iu):
x = np.matrix(d2c (ix)).T
x = np.array(d2c (ix))
u = d2cu(iu)

self.xc,_ = self.pendulum.dynamics(x,u)
Expand Down
6 changes: 3 additions & 3 deletions doc/d-practical-exercises/src/mobilerobot.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,17 +19,17 @@ class MobileRobotWrapper(RobotWrapper):
def __init__(self, urdf, pkgs):
self.initFromURDF(urdf, pkgs, pin.JointModelPlanar())

M0 = pin.SE3(eye(3), np.matrix([.0, .0, .6]).T)
M0 = pin.SE3(eye(3), np.array([.0, .0, .6]))
self.model.jointPlacements[2] = M0 * self.model.jointPlacements[2]
self.visual_model.geometryObjects[0].placement = M0 * self.visual_model.geometryObjects[0].placement
self.visual_data.oMg[0] = M0 * self.visual_data.oMg[0]

# Placement of the "mobile" frame wrt basis center.
basisMop = pin.SE3(eye(3), np.matrix([.3, .0, .1]).T)
basisMop = pin.SE3(eye(3), np.array([.3, .0, .1]))
self.model.addFrame(pin.Frame('mobile', 1, 1, basisMop, pin.FrameType.OP_FRAME))

# Placement of the tool frame wrt end effector frame (located at the center of the wrist)
effMop = pin.SE3(eye(3), np.matrix([.0, .08, .095]).T)
effMop = pin.SE3(eye(3), np.maarraytrix([.0, .08, .095]))
self.model.addFrame(pin.Frame('tool', 6, 6, effMop, pin.FrameType.OP_FRAME))

# Create data again after setting frames
Expand Down
8 changes: 4 additions & 4 deletions doc/d-practical-exercises/src/pendulum.py
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ def createPendulum(self,nbJoint,rootId=0,prefix='',jointPlacement=None):
length = 1.0
mass = length
inertia = pin.Inertia(mass,
np.matrix([0.0,0.0,length/2]).T,
np.array([0.0,0.0,length/2]),
mass/5*np.diagflat([ 1e-2,length**2, 1e-2 ]) )

for i in range(nbJoint):
Expand All @@ -93,8 +93,8 @@ def createPendulum(self,nbJoint,rootId=0,prefix='',jointPlacement=None):
try:self.viewer.viewer.gui.addCapsule('world/'+prefix+'arm'+istr, .1,.8*length,color)
except:pass
self.visuals.append( Visual('world/'+prefix+'arm'+istr,jointId,
pin.SE3(eye(3),np.matrix([0.,0.,length/2]))))
jointPlacement = pin.SE3(eye(3),np.matrix([0.0,0.0,length]).T)
pin.SE3(eye(3),np.array([0.,0.,length/2]))))
jointPlacement = pin.SE3(eye(3),np.array([0.0,0.0,length]))

self.model.addFrame( pin.Frame('tip',jointId,0,jointPlacement,pin.FrameType.OP_FRAME) )

Expand Down Expand Up @@ -156,7 +156,7 @@ def dynamics(self,x,u,display=False):
cost = 0.0
q = modulePi(x[:self.nq])
v = x[self.nq:]
u = np.clip(np.reshape(np.matrix(u),[self.nu,1]),-self.umax,self.umax)
u = np.clip(np.reshape(np.array(u),[self.nu,1]),-self.umax,self.umax)


DT = self.DT/self.NDT
Expand Down
54 changes: 27 additions & 27 deletions doc/d-practical-exercises/src/robot_hand.py
Original file line number Diff line number Diff line change
Expand Up @@ -96,10 +96,10 @@ def collision(self, c2, data=None, oMj1=None, oMj2=None):
wb = pb + ab * r2

# Compute normal matrix
x = np.matrix([1., 0, 0]).T
x = np.array([1., 0, 0])
r1 = cross(ab, x)
if norm(r1) < 1e-2:
x = np.matrix([0, 1., 0]).T
x = np.array([0, 1., 0])
r1 = cross(ab, x)
r1 /= norm(r1)
r2 = cross(ab, r1)
Expand Down Expand Up @@ -158,10 +158,10 @@ def __init__(self):

def createHand(self, root_id=0, prefix='', joint_placement=None):
def trans(x, y, z):
return pin.SE3(eye(3), np.matrix([x, y, z]).T)
return pin.SE3(eye(3), np.array([x, y, z]))

def inertia(m, c):
return pin.Inertia(m, np.matrix(c, np.double).T, eye(3) * m ** 2)
return pin.Inertia(m, np.array(c, np.double), eye(3) * m ** 2)

def joint_name(body):
return prefix + body + '_joint'
Expand All @@ -186,18 +186,18 @@ def body_name(body):
self.visuals.append(Visual(body_name('wpalmb'), joint_id, pin.SE3(rotate('x', pi / 2), zero(3)), H, W))

self.viewer.viewer.gui.addCapsule(body_name('wpalmt'), H, W, color)
pos = pin.SE3(rotate('x', pi / 2), np.matrix([L, 0, 0]).T)
pos = pin.SE3(rotate('x', pi / 2), np.array([L, 0, 0]))
self.visuals.append(Visual(body_name('wpalmt'), joint_id, pos, H, W))

self.viewer.viewer.gui.addCapsule(body_name('wpalml'), H, L, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, -W / 2, 0]).T)
pos = pin.SE3(rotate('y', pi / 2), np.array([L / 2, -W / 2, 0]))
self.visuals.append(Visual(body_name('wpalml'), joint_id, pos, H, L))

self.viewer.viewer.gui.addCapsule(body_name('wpalmr'), H, L, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([L / 2, +W / 2, 0]).T)
pos = pin.SE3(rotate('y', pi / 2), np.array([L / 2, +W / 2, 0]))
self.visuals.append(Visual(body_name('wpalmr'), joint_id, pos, H, L))

joint_placement = pin.SE3(eye(3), np.matrix([5 * cm, 0, 0]).T)
joint_placement = pin.SE3(eye(3), np.array([5 * cm, 0, 0]))
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('palm'))
self.model.appendBodyToJoint(joint_id, inertia(2, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('palm2'), 1 * cm, W, color)
Expand All @@ -206,79 +206,79 @@ def body_name(body):
FL = 4 * cm
palmIdx = joint_id

joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, W / 2, 0]).T)
joint_placement = pin.SE3(eye(3), np.array([2 * cm, W / 2, 0]))
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger11'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger11'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
self.visuals.append(Visual(body_name('finger11'), joint_id, pos, H, FL - 2 * H))

joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_placement = pin.SE3(eye(3), np.array([FL, 0, 0]))
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger12'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())

self.viewer.viewer.gui.addCapsule(body_name('finger12'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
self.visuals.append(Visual(body_name('finger12'), joint_id, pos, H, FL - 2 * H))

joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
joint_placement = pin.SE3(eye(3), np.array([FL - 2 * H, 0, 0]))
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger13'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger13'), H, color)
self.visuals.append(Visual(body_name('finger13'), joint_id, trans(2 * H, 0, 0), H, 0))

joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, 0, 0]).T)
joint_placement = pin.SE3(eye(3), np.array([2 * cm, 0, 0]))
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger21'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger21'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
self.visuals.append(Visual(body_name('finger21'), joint_id, pos, H, FL - 2 * H))

joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_placement = pin.SE3(eye(3), np.array([FL, 0, 0]))
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger22'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger22'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
self.visuals.append(Visual(body_name('finger22'), joint_id, pos, H, FL - 2 * H))

joint_placement = pin.SE3(eye(3), np.matrix([FL - H, 0, 0]).T)
joint_placement = pin.SE3(eye(3), np.array([FL - H, 0, 0]))
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger23'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger23'), H, color)
self.visuals.append(Visual(body_name('finger23'), joint_id, trans(H, 0, 0), H, 0))

joint_placement = pin.SE3(eye(3), np.matrix([2 * cm, -W / 2, 0]).T)
joint_placement = pin.SE3(eye(3), np.array([2 * cm, -W / 2, 0]))
joint_id = self.model.addJoint(palmIdx, pin.JointModelRY(), joint_placement, joint_name('finger31'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger31'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
self.visuals.append(Visual(body_name('finger31'), joint_id, pos, H, FL - 2 * H))

joint_placement = pin.SE3(eye(3), np.matrix([FL, 0, 0]).T)
joint_placement = pin.SE3(eye(3), np.array([FL, 0, 0]))
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger32'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('finger32'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
pos = pin.SE3(rotate('y', pi / 2), np.array([FL / 2 - H, 0, 0]))
self.visuals.append(Visual(body_name('finger32'), joint_id, pos, H, FL - 2 * H))

joint_placement = pin.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
joint_placement = pin.SE3(eye(3), np.array([FL - 2 * H, 0, 0]))
joint_id = self.model.addJoint(joint_id, pin.JointModelRY(), joint_placement, joint_name('finger33'))
self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addSphere(body_name('finger33'), H, color)
self.visuals.append(Visual(body_name('finger33'), joint_id, trans(2 * H, 0, 0), H, 0))

joint_placement = pin.SE3(eye(3), np.matrix([1 * cm, -W / 2 - H * 1.5, 0]).T)
joint_placement = pin.SE3(eye(3), np.array([1 * cm, -W / 2 - H * 1.5, 0]))
joint_id = self.model.addJoint(1, pin.JointModelRY(), joint_placement, joint_name('thumb1'))
self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('thumb1'), H, 2 * cm, color)
pos = pin.SE3(rotate('z', pi / 3) * rotate('x', pi / 2), np.matrix([1 * cm, -1 * cm, 0]).T)
pos = pin.SE3(rotate('z', pi / 3) * rotate('x', pi / 2), np.array([1 * cm, -1 * cm, 0]))
self.visuals.append(Visual(body_name('thumb1'), joint_id, pos, 2 * cm))

joint_placement = pin.SE3(rotate('z', pi / 3) * rotate('x', pi), np.matrix([3 * cm, -1.8 * cm, 0]).T)
joint_placement = pin.SE3(rotate('z', pi / 3) * rotate('x', pi), np.array([3 * cm, -1.8 * cm, 0]))
joint_id = self.model.addJoint(joint_id, pin.JointModelRZ(), joint_placement, joint_name('thumb2'))
self.model.appendBodyToJoint(joint_id, inertia(.4, [0, 0, 0]), pin.SE3.Identity())
self.viewer.viewer.gui.addCapsule(body_name('thumb2'), H, FL - 2 * H, color)
pos = pin.SE3(rotate('x', pi / 3), np.matrix([-0.7 * cm, .8 * cm, -0.5 * cm]).T)
pos = pin.SE3(rotate('x', pi / 3), np.array([-0.7 * cm, .8 * cm, -0.5 * cm]))
self.visuals.append(Visual(body_name('thumb2'), joint_id, pos, H, FL - 2 * H))

# Prepare some patches to represent collision points. Yet unvisible.
Expand Down
6 changes: 3 additions & 3 deletions doc/d-practical-exercises/src/ur5x4.py
Original file line number Diff line number Diff line change
Expand Up @@ -31,12 +31,12 @@ def loadRobot(M0, name):

robots = []
# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
Mt = SE3(eye(3), np.matrix([.3, 0, 0]).T) # First robot is simply translated
Mt = SE3(eye(3), np.array([.3, 0, 0])) # First robot is simply translated
for i in range(4):
robots.append(loadRobot(SE3(rotate('z', np.pi / 2 * i), zero(3)) * Mt, "robot%d" % i))

# Set up the robots configuration with end effector pointed upward.
q0 = np.matrix([np.pi / 4, -np.pi / 4, -np.pi / 2, np.pi / 4, np.pi / 2, 0]).T
q0 = np.array([np.pi / 4, -np.pi / 4, -np.pi / 2, np.pi / 4, np.pi / 2, 0])
for i in range(4):
robots[i].display(q0)

Expand All @@ -45,6 +45,6 @@ def loadRobot(M0, name):
w, h, d = 0.25, 0.25, 0.005
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
gepettoViewer.addBox('world/toolplate', w, h, d, color)
Mtool = SE3(rotate('z', 1.268), np.matrix([0, 0, .77]).T)
Mtool = SE3(rotate('z', 1.268), np.array([0, 0, .77]))
gepettoViewer.applyConfiguration('world/toolplate', se3ToXYZQUATtuple(Mtool))
gepettoViewer.refresh()
4 changes: 2 additions & 2 deletions examples/forward-dynamics-derivatives.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@
model.upperPositionLimit = np.ones((model.nq,1))

q = pin.randomConfiguration(model) # joint configuration
v = np.matrix(np.random.rand(model.nv,1)) # joint velocity
tau = np.matrix(np.random.rand(model.nv,1)) # joint acceleration
v = np.random.rand(model.nv,1) # joint velocity
tau = np.random.rand(model.nv,1) # joint acceleration

# Evaluate the derivatives

Expand Down
2 changes: 1 addition & 1 deletion models/simple_model.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@

def placement(x=0, y=0, z=0, rx=0, ry=0, rz=0):
m = pin.SE3.Identity()
m.translation = np.matrix([[float(i)] for i in [x, y, z]])
m.translation = np.array([x, y, z])
m.rotation *= rotate('x', rx)
m.rotation *= rotate('y', ry)
m.rotation *= rotate('z', rz)
Expand Down
4 changes: 2 additions & 2 deletions unittest/python/bindings_com_velocity_derivatives.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@

def df_dq(model,func,q,h=1e-9):
""" Perform df/dq by num_diff. q is in the lie manifold.
:params func: function to differentiate f : np.matrix -> np.matrix
:params q: configuration value at which f is differentiated. type np.matrix
:params func: function to differentiate f : np.array -> np.array
:params q: configuration value at which f is differentiated. type np.array
:params h: eps
:returns df/dq
Expand Down

0 comments on commit 08d6c5e

Please sign in to comment.