Commit 88129e41 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

flake8

parent 82805554
Pipeline #17304 passed with stage
in 16 seconds
......@@ -43,7 +43,7 @@ class CallbackLogger:
self.nfeval = 1
def __call__(self, x):
print('===CBK=== {0:4d} {1: 3.6f} {2: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x)))
print('===CBK=== {0:4d} {1: 3.6f} {2: 3.6f} {3: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x)))
self.nfeval += 1
......
......@@ -6,5 +6,5 @@ p1 = np.random.rand(3)
for t in np.arange(0, 1, .01):
p = p0 * (1 - t) + p1 * t
viz.applyConfiguration('world/box', list(p) + list(quat.coeffs()))
viz.applyConfiguration('world/box', list(p) + list(quat.coeffs())) # noqa: F821 TODO
time.sleep(.01)
......@@ -24,11 +24,11 @@ def nlerp(q0, q1, t):
q0 = Quaternion(SE3.Random().rotation)
q1 = Quaternion(SE3.Random().rotation)
viz.applyConfiguration('world/box', [0, 0, 0] + list(q0.coeffs()))
viz.applyConfiguration('world/box', [0, 0, 0] + list(q0.coeffs())) # noqa: F821 TODO
sleep(.1)
for t in np.arange(0, 1, .01):
q = nlerp(q0, q1, t)
viz.applyConfiguration('world/box', [0, 0, 0] + list(q.coeffs()))
viz.applyConfiguration('world/box', [0, 0, 0] + list(q.coeffs())) # noqa: F821 TODO
sleep(.01)
sleep(.1)
viz.applyConfiguration('world/box', [0, 0, 0] + list(q1.coeffs()))
viz.applyConfiguration('world/box', [0, 0, 0] + list(q1.coeffs())) # noqa: F821 TODO
......@@ -277,7 +277,7 @@
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"display_name": "Python 3 (ipykernel)",
"language": "python",
"name": "python3"
},
......@@ -291,7 +291,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.9"
"version": "3.10.1"
}
},
"nbformat": 4,
......
from IPython.core import magic_arguments
from IPython.core.magic import line_magic, cell_magic, line_cell_magic, Magics, magics_class
from IPython.core.magic import line_magic, Magics, magics_class
@magics_class
......@@ -9,7 +8,7 @@ class DoNotLoadMagics(Magics):
@line_magic
def do_not_load(self, line):
if DoNotLoadMagics.forceLoad:
get_ipython().run_line_magic('load', line)
get_ipython().run_line_magic('load', line) # noqa: F821
@line_magic
def force_load(self, line):
......@@ -21,7 +20,7 @@ class DoNotLoadMagics(Magics):
print('Force load is OFF')
ip = get_ipython()
ip = get_ipython() # noqa: F821
ip.register_magics(DoNotLoadMagics)
......
......@@ -3,8 +3,8 @@ import pinocchio as pin
from utils.meshcat_viewer_wrapper import MeshcatVisualizer
import time
import numpy as np
from numpy.linalg import inv, norm, pinv, svd, eig
from scipy.optimize import fmin_bfgs, fmin_slsqp
from numpy.linalg import norm
from scipy.optimize import fmin_slsqp
from utils.load_ur5_with_obstacles import load_ur5_with_obstacles, Target
import matplotlib.pylab as plt
# %end_jupyter_snippet
......@@ -66,7 +66,8 @@ def qrand(check=False):
'''
while True:
q = np.random.rand(2) * 6.4 - 3.2 # sample between -3.2 and +3.2.
if not check or not coll(q): return q
if not check or not coll(q):
return q
# %end_jupyter_snippet
......@@ -76,7 +77,8 @@ def qrand(check=False):
def collisionDistance(q):
'''Return the minimal distance between robot and environment. '''
pin.updateGeometryPlacements(robot.model, robot.data, robot.collision_model, robot.collision_data, q)
if pin.computeCollisions(robot.collision_model, robot.collision_data, False): 0
if pin.computeCollisions(robot.collision_model, robot.collision_data, False):
return 0
idx = pin.computeDistances(robot.collision_model, robot.collision_data)
return robot.collision_data.distanceResults[idx].min_distance
......@@ -116,7 +118,6 @@ def randomDescent(q0=None):
hist = [q.copy()]
for i in range(100):
dq = qrand() * .1 # Choose a random step ...
qtry = q + dq # ... apply
if dist(q) > dist(q + dq) and not coll(q + dq): # If distance decrease without collision ...
q = q + dq # ... keep the step
hist.append(q.copy()) # ... keep a trace of it
......@@ -154,8 +155,8 @@ def sampleSpace(nbSamples=500):
def plotConfigurationSpace(hcol, hfree, markerSize=20):
'''
Plot 2 "scatter" plots: the first one plot the distance to the target for
each configuration, the second plots the distance to the obstacles (axis q1,q2,
Plot 2 "scatter" plots: the first one plot the distance to the target for
each configuration, the second plots the distance to the obstacles (axis q1,q2,
distance in the color space).
'''
htotal = hcol + hfree
......@@ -178,7 +179,7 @@ plotConfigurationSpace(hcol, hfree)
################################################################################
################################################################################
### Plot random trajectories in the same plot
# Plot random trajectories in the same plot
# %jupyter_snippet traj
qinit = np.array([-1.1, -3.])
for i in range(100):
......@@ -187,9 +188,9 @@ for i in range(100):
print('We found a good traj!')
break
traj = np.array(traj)
### Chose trajectory end to be in [-pi,pi]
# Chose trajectory end to be in [-pi,pi]
qend = (traj[-1] + np.pi) % (2 * np.pi) - np.pi
### Take the entire trajectory it modulo 2 pi
# Take the entire trajectory it modulo 2 pi
traj += (qend - traj[-1])
# %end_jupyter_snippet
plt.plot(traj[:, 0], traj[:, 1], 'r', lw=5)
......@@ -225,7 +226,7 @@ def callback(q):
def optimize():
'''
Optimize from an initial random configuration to discover a collision-free
configuration as close as possible to the target.
configuration as close as possible to the target.
'''
return fmin_slsqp(x0=qrand(check=True), func=cost, f_ieqcons=constraint, callback=callback, full_output=1)
......
......@@ -7,7 +7,7 @@ import time
import numpy as np
from scipy.optimize import fmin_bfgs, fmin_slsqp
from utils.meshcat_viewer_wrapper import MeshcatVisualizer, translation2d, planar
from numpy.linalg import norm, inv, pinv, svd, eig
from numpy.linalg import norm
viz = MeshcatVisualizer(url='classical')
......
......@@ -6,9 +6,8 @@ scipy BFGS.
# %jupyter_snippet import
import time
import numpy as np
from scipy.optimize import fmin_bfgs, fmin_slsqp
from scipy.optimize import fmin_bfgs
from utils.meshcat_viewer_wrapper import MeshcatVisualizer, translation2d, planar
from numpy.linalg import norm, inv, pinv, svd, eig
# %end_jupyter_snippet
viz = MeshcatVisualizer(url='classical')
......
......@@ -64,7 +64,7 @@ qopt = fmin_bfgs(cost, robot.q0, callback=callback)
##########################################################################
### This last part is to automatically validate the versions of this example.
# This last part is to automatically validate the versions of this example.
class InvGeom3DTest(unittest.TestCase):
def test_qopt_translation(self):
......
......@@ -86,7 +86,7 @@ viz.display(q0)
# %jupyter_snippet 5
# Add a red box in the viewer
boxID = "world/box"
#viz.delete(ballID)
# viz.delete(ballID)
viz.addBox(boxID, [0.1, 0.2, 0.1], colors.magenta)
# Place the box at the position (0.5, 0.1, 0.2)
......@@ -134,7 +134,7 @@ for i in range(100):
##########################################################################
### This last part is to automatically validate the versions of this example.
# This last part is to automatically validate the versions of this example.
class SimplePickAndPlaceTest(unittest.TestCase):
def test_oMbox_translation(self):
......
......@@ -6,13 +6,14 @@ Template of the program for TP3
import pinocchio as pin
import numpy as np
import time
from numpy.linalg import pinv, inv, norm, svd, eig
from numpy.linalg import pinv
from tiago_loader import loadTiago
import matplotlib.pylab as plt
plt.ion()
from tp3.meshcat_viewer_wrapper import MeshcatVisualizer
plt.ion()
robot = loadTiago(addGazeFrame=True)
viz = MeshcatVisualizer(robot, url='classical')
......
......@@ -6,13 +6,14 @@ Template of the program for TP3
import pinocchio as pin
import numpy as np
import time
from numpy.linalg import pinv, inv, norm, svd, eig
from numpy.linalg import pinv
from tiago_loader import loadTiago
import matplotlib.pylab as plt
plt.ion()
from tp3.meshcat_viewer_wrapper import MeshcatVisualizer
plt.ion()
robot = loadTiago()
viz = MeshcatVisualizer(robot, url='classical')
......
import numpy as np
import pinocchio as pin
import example_robot_data as robex
from example_robot_data.robots_loader import TiagoLoader
import hppfcl
from os.path import dirname, exists, join
class TiagoLoader(object):
#path = ''
#urdf_filename = ''
srdf_filename = ''
urdf_subpath = 'robots'
srdf_subpath = 'srdf'
ref_posture = 'half_sitting'
has_rotor_parameters = False
free_flyer = True
verbose = False
path = "tiago_description"
urdf_filename = "tiago_no_hand.urdf"
def __init__(self):
urdf_path = join(self.path, self.urdf_subpath, self.urdf_filename)
self.model_path = robex.getModelPath(urdf_path, self.verbose)
self.urdf_path = join(self.model_path, urdf_path)
self.robot = pin.RobotWrapper.BuildFromURDF(self.urdf_path, [join(self.model_path, '../..')],
pin.JointModelPlanar() if self.free_flyer else None)
if self.srdf_filename:
self.srdf_path = join(self.model_path, self.path, self.srdf_subpath, self.srdf_filename)
self.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose, self.has_rotor_parameters,
self.ref_posture)
else:
self.srdf_path = None
self.q0 = None
if self.free_flyer:
self.addFreeFlyerJointLimits()
def addFreeFlyerJointLimits(self):
ub = self.robot.model.upperPositionLimit
ub[:self.robot.model.joints[1].nq] = 1
self.robot.model.upperPositionLimit = ub
lb = self.robot.model.lowerPositionLimit
lb[:self.robot.model.joints[1].nq] = -1
self.robot.model.lowerPositionLimit = lb
def loadTiago(addGazeFrame=False):
def load_tiago(addGazeFrame=False):
'''
Load a tiago model, without the hand, and with the two following modifications wrt example_robot_data.
- first, the first joint is a planar (x,y,cos,sin) joint, while it is a fixed robot in example robot data.
- second, two visual models of a frame have been added to two new op-frame, "tool0" on the robot hand, and "basis0" in
front of the basis.
- second, two visual models of a frame have been added to two new op-frame, "tool0" on the robot hand, and "basis0"
in front of the basis.
'''
robot = TiagoLoader().robot
......@@ -141,7 +100,7 @@ def loadTiago(addGazeFrame=False):
if __name__ == "__main__":
from tp2.meshcat_viewer_wrapper import MeshcatVisualizer
robot = loadTiago()
robot = load_tiago()
viz = MeshcatVisualizer(robot, url='classical')
viz.display(robot.q0)
......@@ -74,20 +74,26 @@ class CollisionWrapper:
def getCollisionJacobian(self, collisions=None):
'''From a collision list, return the Jacobian corresponding to the normal direction. '''
if collisions is None: collisions = self.getCollisionList()
if len(collisions) == 0: return np.ndarray([0, self.rmodel.nv])
if collisions is None:
collisions = self.getCollisionList()
if len(collisions) == 0:
return np.ndarray([0, self.rmodel.nv])
J = np.vstack([self._getCollisionJacobian(c, r) for (i, c, r) in collisions])
return J
def getCollisionJdotQdot(self, collisions=None):
if collisions is None: collisions = self.getCollisionList()
if len(collisions) == 0: return np.array([])
if collisions is None:
collisions = self.getCollisionList()
if len(collisions) == 0:
return np.array([])
a0 = np.vstack([self._getCollisionJdotQdot(c, r) for (i, c, r) in collisions])
return a0
def getCollisionDistances(self, collisions=None):
if collisions is None: collisions = self.getCollisionList()
if len(collisions) == 0: return np.array([])
if collisions is None:
collisions = self.getCollisionList()
if len(collisions) == 0:
return np.array([])
dist = np.array([self.gdata.distanceResults[i].min_distance for (i, c, r) in collisions])
return dist
......@@ -96,7 +102,8 @@ class CollisionWrapper:
# --- DISPLAY -----------------------------------------------------------------------------------
def initDisplay(self, viz=None):
if viz is not None: self.viz = viz
if viz is not None:
self.viz = viz
assert (self.viz is not None)
self.patchName = 'world/contact_%d_%s'
......@@ -105,7 +112,8 @@ class CollisionWrapper:
def createDisplayPatchs(self, ncollisions):
if ncollisions == self.ncollisions: return
if ncollisions == self.ncollisions:
return
elif ncollisions < self.ncollisions: # Remove patches
for i in range(ncollisions, self.ncollisions):
viz[self.patchName % (i, 'a')].delete()
......@@ -113,15 +121,15 @@ class CollisionWrapper:
else:
for i in range(self.ncollisions, ncollisions):
viz.addCylinder(self.patchName % (i, 'a'), .0005, .005, "red")
#viz.addCylinder( self.patchName % (i,'b') , .0005,.05,"red")
# viz.addCylinder( self.patchName % (i,'b') , .0005,.05,"red")
self.ncollisions = ncollisions
def displayContact(self, ipatch, contact):
'''
Display a small red disk at the position of the contact, perpendicular to the
contact normal.
contact normal.
@param ipatchf: use patch named "world/contact_%d" % contactRef.
@param contact: the contact object, taken from Pinocchio (HPP-FCL) e.g.
geomModel.collisionResults[0].geContact(0).
......@@ -133,8 +141,10 @@ class CollisionWrapper:
def displayCollisions(self, collisions=None):
'''Display in the viewer the collision list get from getCollisionList().'''
if self.viz is None: return
if collisions is None: collisions = self.getCollisionList()
if self.viz is None:
return
if collisions is None:
collisions = self.getCollisionList()
self.createDisplayPatchs(len(collisions))
for ic, [i, c, r] in enumerate(collisions):
......@@ -144,14 +154,12 @@ class CollisionWrapper:
if __name__ == "__main__":
from tp6.meshcat_viewer_wrapper import MeshcatVisualizer
from tp6.robot_hand import RobotHand
import time
import numpy as np
from numpy import cos, sin
np.random.seed(3)
robot = RobotHand()
viz = MeshcatVisualizer(robot, url='tcp://127.0.0.1:6000') #classical')
viz = MeshcatVisualizer(robot, url='tcp://127.0.0.1:6000') # classical')
q = robot.q0.copy()
q[0] = .5
......@@ -167,7 +175,7 @@ if __name__ == "__main__":
ci = cols[0][2]
col.displayContact(0, ci.getContact(0))
### Try to find a random contact
# Try to find a random contact
if 0:
q = robot.q0.copy()
vq = np.random.rand(robot.model.nv) * 2 - 1
......@@ -175,8 +183,10 @@ if __name__ == "__main__":
q += vq * 1e-3
col.computeCollisions(q)
cols = col.getCollisionList()
if len(cols) > 0: break
if not i % 20: viz.display(q)
if len(cols) > 0:
break
if not i % 20:
viz.display(q)
viz.display(q)
......@@ -192,7 +202,7 @@ if __name__ == "__main__":
[
col.gdata.oMg[11],
col.gdata.oMg[3],
#col.gmodel.geometryObjects[11].geometry,
# col.gmodel.geometryObjects[11].geometry,
],
file)
......
......@@ -5,10 +5,10 @@ sudo apt install robotpkg-py35-quadprog
import quadprog
import numpy as np
from numpy.linalg import inv, pinv, norm, eig, svd
from numpy.linalg import eig
A = np.random.rand(5, 5) * 2 - 1
A = A @ A.T ### Make it positive symmetric
A = A @ A.T # Make it positive symmetric
b = np.random.rand(5)
C = np.random.rand(10, 5)
......@@ -22,4 +22,4 @@ assert (np.isclose(x @ A @ x / 2 - b @ x, cost))
assert (np.all((C @ x - d) >= -1e-6)) # Check primal KKT condition
assert (np.all(np.isclose((C @ x - d)[iact - 1], 0))) # Check primal complementarity
assert (np.all(np.isclose((A @ x - b - lag @ C), 0))) # Check dual KKT condition
assert (np.all(np.isclose(lag[[not i in iact - 1 for i in range(len(lag))]], 0))) # Check dual complementarity
assert (np.all(np.isclose(lag[[i not in iact - 1 for i in range(len(lag))]], 0))) # Check dual complementarity
from pinocchio.explog import exp, log
from numpy.linalg import pinv, norm, pinv
from pinocchio.utils import rotate
import pinocchio as pin
import time
import numpy as np
from numpy import pi
from numpy import cos, sin, pi, hstack, vstack, argmin
import pinocchio as pin
from numpy import sin, pi
import hppfcl
def Capsule(name, joint, radius, length, placement, color=[.7, .7, 0.98, 1]):
'''Create a Pinocchio::FCL::Capsule to be added in the Geom-Model. '''
### They should be capsules ... but hppfcl current version is buggy with Capsules...
#hppgeom = hppfcl.Capsule(radius,length)
# They should be capsules ... but hppfcl current version is buggy with Capsules...
# hppgeom = hppfcl.Capsule(radius, length)
hppgeom = hppfcl.Cylinder(radius, length)
geom = pin.GeometryObject(name, joint, hppgeom, placement)
geom.meshColor = np.array(color)
......@@ -28,23 +26,23 @@ def Sphere(name, joint, radius, placement, color=[.9, .9, 0.98, 1]):
class RobotHand:
'''
Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3).
The configuration is nq=7. The velocity is the same.
Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3).
The configuration is nq=7. The velocity is the same.
The members of the class are:
* viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them.
* model: the kinematic tree of the robot.
* data: the temporary variables to be used by the kinematic algorithms.
* visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being
an object Visual (see above).
CollisionPairs is a list of visual indexes.
CollisionPairs is a list of visual indexes.
Reference to the collision pair is used in the collision test and jacobian of the collision
(which are simply proxy method to methods of the visual class).
'''
def __init__(self):
self.viewer = None
#self.visuals = []
# self.visuals = []
self.model = pin.Model()
self.gmodel = pin.GeometryModel()
......@@ -56,7 +54,7 @@ class RobotHand:
self.gdata.collisionRequests.enable_contact = True
self.q0 = np.zeros(self.model.nq)
#self.q0[0] = np.pi
# self.q0[0] = np.pi
self.q0[-2] = np.pi / 3
self.q0[2:-4] = np.pi / 6
self.q0[11:] = np.pi / 4
......@@ -125,27 +123,31 @@ class RobotHand:
caps = Capsule(name, joint, radius * 0.99, length, placement)
caps.meshColor = np.array([1.] * 4)
self.gmodel.addGeometryObject(caps)
#self.gmodel.geometryObjects[-1].meshColor = np.array([1.]*4)
# self.gmodel.geometryObjects[-1].meshColor = np.array([1.]*4)
def createHand(self, rootId=0, jointPlacement=None):
color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
colorred = [1.0, 0.0, 0.0, 1.0]
# color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
# colorred = [1.0, 0.0, 0.0, 1.0]
jointId = rootId
cm = 1e-2
trans = lambda x, y, z: pin.SE3(np.eye(3), np.array([x, y, z]))
inertia = lambda m, c: pin.Inertia(m, np.array(c, np.double), np.eye(3) * m**2)
def trans(x, y, z):
return pin.SE3(np.eye(3), np.array([x, y, z]))
def inertia(m, c):
return pin.Inertia(m, np.array(c, np.double), np.eye(3) * m**2)
name = "wrist"
jointName, bodyName = [name + "_joint", name + "_body"]
#jointPlacement = jointPlacement if jointPlacement!=None else pin.SE3.Identity()
jointPlacement = jointPlacement if jointPlacement != None else pin.SE3(pin.utils.rotate('y', np.pi),
np.zeros(3))
jointName = name + "_joint"
# jointPlacement = jointPlacement if jointPlacement!=None else pin.SE3.Identity()
jointPlacement = jointPlacement if jointPlacement is not None else pin.SE3(pin.utils.rotate('y', np.pi),
np.zeros(3))
jointId = self.model.addJoint(jointId, pin.JointModelRY(), jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(3, [0, 0, 0]), pin.SE3.Identity())
## Hand dimsensions: length, width, height(=depth), finger-length
# Hand dimsensions: length, width, height(=depth), finger-length
L = 3 * cm
W = 5 * cm
H = 1 * cm
......@@ -174,7 +176,7 @@ class RobotHand:
color=[.7, .7, 0.98, 1]))
name = "palm"
jointName, bodyName = [name + "_joint", name + "_body"]
jointName = name + "_joint"
jointPlacement = pin.SE3(np.eye(3), np.array([5 * cm, 0, 0]))
jointId = self.model.addJoint(jointId, pin.JointModelRY(), jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(2, [0, 0, 0]), pin.SE3.Identity())
......@@ -183,7 +185,7 @@ class RobotHand:
palmIdx = jointId
name = "finger11"
jointName, bodyName = [name + "_joint", name + "_body"]
jointName = name + "_joint"
jointPlacement = pin.SE3(np.eye(3), np.array([2 * cm, W / 2, 0]))
jointId = self.model.addJoint(palmIdx, pin.JointModelRY(), jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
......@@ -192,7 +194,7 @@ class RobotHand:
0]))))
name = "finger12"
jointName, bodyName = [name + "_joint", name + "_body"]
jointName = name + "_joint"
jointPlacement = pin.SE3(np.eye(3), np.array([FL, 0, 0]))
jointId = self.model.addJoint(jointId, pin.JointModelRY(), jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.5, [0, 0, 0]), pin.SE3.Identity())
......@@ -201,14 +203,14 @@ class RobotHand:
0]))))
name = "finger13"
jointName, bodyName = [name + "_joint", name + "_body"]
jointName = name + "_joint"
jointPlacement = pin.SE3(np.eye(3), np.array([FL - H, 0, 0]))
jointId = self.model.addJoint(jointId, pin.JointModelRY(), jointPlacement, jointName)
self.model.appendBodyToJoint(jointId, inertia(.3, [0, 0, 0]), pin.SE3.Identity())
self.gmodel.addGeometryObject(Sphere('world/finger13', jointId, H, trans(H, 0, 0)))
name = "finger21"
jointName, bodyName = [name + "_joint", name + "_body"]