From 08d6c5e6a2b98f2fcf7f73b2fbd1cbefeb32cd52 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Fri, 28 Apr 2023 20:50:57 +0200 Subject: [PATCH] python: fix other np.matrix issues --- bindings/python/pinocchio/derivative/dcrba.py | 6 +-- .../python/pinocchio/derivative/lambdas.py | 2 +- bindings/python/pinocchio/romeo_wrapper.py | 2 - doc/d-practical-exercises/1-directgeom.md | 2 +- doc/d-practical-exercises/2-invgeom.md | 2 +- doc/d-practical-exercises/3-invkine.md | 2 +- doc/d-practical-exercises/5-planner.md | 2 +- doc/d-practical-exercises/6-wpg.md | 4 +- doc/d-practical-exercises/src/dpendulum.py | 4 +- doc/d-practical-exercises/src/mobilerobot.py | 6 +-- doc/d-practical-exercises/src/pendulum.py | 8 +-- doc/d-practical-exercises/src/robot_hand.py | 54 +++++++++---------- doc/d-practical-exercises/src/ur5x4.py | 6 +-- examples/forward-dynamics-derivatives.py | 4 +- models/simple_model.py | 2 +- .../bindings_com_velocity_derivatives.py | 4 +- 16 files changed, 54 insertions(+), 56 deletions(-) diff --git a/bindings/python/pinocchio/derivative/dcrba.py b/bindings/python/pinocchio/derivative/dcrba.py index abdfd8213a..1fb9f8faaf 100644 --- a/bindings/python/pinocchio/derivative/dcrba.py +++ b/bindings/python/pinocchio/derivative/dcrba.py @@ -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 TiSd=0 @@ -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 diff --git a/bindings/python/pinocchio/derivative/lambdas.py b/bindings/python/pinocchio/derivative/lambdas.py index 2091d4944f..7815c99f5e 100644 --- a/bindings/python/pinocchio/derivative/lambdas.py +++ b/bindings/python/pinocchio/derivative/lambdas.py @@ -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): diff --git a/bindings/python/pinocchio/romeo_wrapper.py b/bindings/python/pinocchio/romeo_wrapper.py index 39ba3a10e8..f98ee721ac 100644 --- a/bindings/python/pinocchio/romeo_wrapper.py +++ b/bindings/python/pinocchio/romeo_wrapper.py @@ -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", diff --git a/doc/d-practical-exercises/1-directgeom.md b/doc/d-practical-exercises/1-directgeom.md index 1e40b5ee18..2725b89eb6 100644 --- a/doc/d-practical-exercises/1-directgeom.md +++ b/doc/d-practical-exercises/1-directgeom.md @@ -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. ``` diff --git a/doc/d-practical-exercises/2-invgeom.md b/doc/d-practical-exercises/2-invgeom.md index a1737477ae..3c80d35ff3 100644 --- a/doc/d-practical-exercises/2-invgeom.md +++ b/doc/d-practical-exercises/2-invgeom.md @@ -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] ``` diff --git a/doc/d-practical-exercises/3-invkine.md b/doc/d-practical-exercises/3-invkine.md index 357a227f26..cf2331a4eb 100644 --- a/doc/d-practical-exercises/3-invkine.md +++ b/doc/d-practical-exercises/3-invkine.md @@ -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) ``` diff --git a/doc/d-practical-exercises/5-planner.md b/doc/d-practical-exercises/5-planner.md index 7f74b3672b..6f3acedc32 100644 --- a/doc/d-practical-exercises/5-planner.md +++ b/doc/d-practical-exercises/5-planner.md @@ -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. diff --git a/doc/d-practical-exercises/6-wpg.md b/doc/d-practical-exercises/6-wpg.md index 537a2b8841..3c558a653b 100644 --- a/doc/d-practical-exercises/6-wpg.md +++ b/doc/d-practical-exercises/6-wpg.md @@ -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) ``` diff --git a/doc/d-practical-exercises/src/dpendulum.py b/doc/d-practical-exercises/src/dpendulum.py index 0b33a68c47..97b82a5017 100644 --- a/doc/d-practical-exercises/src/dpendulum.py +++ b/doc/d-practical-exercises/src/dpendulum.py @@ -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) diff --git a/doc/d-practical-exercises/src/mobilerobot.py b/doc/d-practical-exercises/src/mobilerobot.py index c7cf32d1a7..59155b65da 100644 --- a/doc/d-practical-exercises/src/mobilerobot.py +++ b/doc/d-practical-exercises/src/mobilerobot.py @@ -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 diff --git a/doc/d-practical-exercises/src/pendulum.py b/doc/d-practical-exercises/src/pendulum.py index 43519c705b..4cc9824141 100644 --- a/doc/d-practical-exercises/src/pendulum.py +++ b/doc/d-practical-exercises/src/pendulum.py @@ -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): @@ -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) ) @@ -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 diff --git a/doc/d-practical-exercises/src/robot_hand.py b/doc/d-practical-exercises/src/robot_hand.py index 440090dd96..cdeb24ca2d 100644 --- a/doc/d-practical-exercises/src/robot_hand.py +++ b/doc/d-practical-exercises/src/robot_hand.py @@ -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) @@ -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' @@ -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) @@ -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. diff --git a/doc/d-practical-exercises/src/ur5x4.py b/doc/d-practical-exercises/src/ur5x4.py index c02ef9592f..097f52f538 100644 --- a/doc/d-practical-exercises/src/ur5x4.py +++ b/doc/d-practical-exercises/src/ur5x4.py @@ -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) @@ -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() diff --git a/examples/forward-dynamics-derivatives.py b/examples/forward-dynamics-derivatives.py index 1dd32a1ca3..db197b95af 100644 --- a/examples/forward-dynamics-derivatives.py +++ b/examples/forward-dynamics-derivatives.py @@ -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 diff --git a/models/simple_model.py b/models/simple_model.py index c70aa1f273..d688238936 100644 --- a/models/simple_model.py +++ b/models/simple_model.py @@ -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) diff --git a/unittest/python/bindings_com_velocity_derivatives.py b/unittest/python/bindings_com_velocity_derivatives.py index e9f1c020d3..a11582bb04 100644 --- a/unittest/python/bindings_com_velocity_derivatives.py +++ b/unittest/python/bindings_com_velocity_derivatives.py @@ -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