diff --git a/examples/display_robot.py b/examples/display_robot.py index 76c9953..08b9445 100644 --- a/examples/display_robot.py +++ b/examples/display_robot.py @@ -1,5 +1,4 @@ import example_robot_data as robex - from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer ROBOT_NAME = "solo12" diff --git a/next/appendix/solution_slerp.py b/next/appendix/solution_slerp.py index 5694e24..86861c5 100644 --- a/next/appendix/solution_slerp.py +++ b/next/appendix/solution_slerp.py @@ -4,7 +4,6 @@ import numpy as np from numpy.linalg import norm from pinocchio import SE3, AngleAxis, Quaternion - from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer viz = MeshcatVisualizer() diff --git a/next/tp1/configuration_extended.py b/next/tp1/configuration_extended.py index 5464ff2..54b38b5 100644 --- a/next/tp1/configuration_extended.py +++ b/next/tp1/configuration_extended.py @@ -9,7 +9,6 @@ import numpy as np from numpy.linalg import norm from scipy.optimize import fmin_bfgs, fmin_slsqp - from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer from supaero2024.meshcat_viewer_wrapper.transformations import planar, translation2d diff --git a/next/tp1/configuration_reduced.py b/next/tp1/configuration_reduced.py index 7111481..415f082 100644 --- a/next/tp1/configuration_reduced.py +++ b/next/tp1/configuration_reduced.py @@ -8,7 +8,6 @@ import numpy as np from scipy.optimize import fmin_bfgs - from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer from supaero2024.meshcat_viewer_wrapper.transformations import planar, translation2d diff --git a/next/tp2/floating.py b/next/tp2/floating.py index c47fc61..3c523b0 100644 --- a/next/tp2/floating.py +++ b/next/tp2/floating.py @@ -14,10 +14,8 @@ import example_robot_data as robex import numpy as np -import pinocchio as pin from numpy.linalg import norm from scipy.optimize import fmin_bfgs - from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer # --- Load robot model @@ -88,6 +86,7 @@ def callback(q): viz.display(q) time.sleep(1e-2) + qopt = fmin_bfgs(cost, robot.q0, callback=callback) # %end_jupyter_snippet diff --git a/next/tp2/invgeom3d.py b/next/tp2/invgeom3d.py index c8ebf66..d48505d 100644 --- a/next/tp2/invgeom3d.py +++ b/next/tp2/invgeom3d.py @@ -14,7 +14,6 @@ import numpy as np from numpy.linalg import norm from scipy.optimize import fmin_bfgs - from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer # --- Load robot model diff --git a/next/tp2/invgeom6d.py b/next/tp2/invgeom6d.py index a1979ff..adac38e 100644 --- a/next/tp2/invgeom6d.py +++ b/next/tp2/invgeom6d.py @@ -1,5 +1,5 @@ """ -Stand-alone inverse geom in 6D (controlling the placement, ie translation and +Stand-alone inverse geom in 6D (controlling the placement, ie translation and orientation of the end effector). Given a reference placement , it computes the configuration of the UR5 so that the end-effector placement (6D) matches the target. This is done using BFGS solver. While iterating to compute @@ -16,7 +16,6 @@ import pinocchio as pin from numpy.linalg import norm from scipy.optimize import fmin_bfgs - from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer # --- Load robot model diff --git a/next/tp2/simple_pick_and_place.py b/next/tp2/simple_pick_and_place.py index 3729390..f83724f 100644 --- a/next/tp2/simple_pick_and_place.py +++ b/next/tp2/simple_pick_and_place.py @@ -14,7 +14,6 @@ import example_robot_data as robex import numpy as np import pinocchio as pin - from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer, colors # %jupyter_snippet 1 diff --git a/next/tp3/control_head.py b/next/tp3/control_head.py index bfeed76..1a2fbae 100644 --- a/next/tp3/control_head.py +++ b/next/tp3/control_head.py @@ -12,9 +12,8 @@ import numpy as np import pinocchio as pin from numpy.linalg import norm, pinv -from tp3.tiago_loader import loadTiago - from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer +from tp3.tiago_loader import loadTiago plt.ion() diff --git a/next/tp3/inverse_kinematics.py b/next/tp3/inverse_kinematics.py index fb57c42..deb6583 100644 --- a/next/tp3/inverse_kinematics.py +++ b/next/tp3/inverse_kinematics.py @@ -14,9 +14,8 @@ # %jupyter_snippet import import pinocchio as pin from numpy.linalg import norm, pinv -from tp3.tiago_loader import loadTiago - from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer +from tp3.tiago_loader import loadTiago # %end_jupyter_snippet diff --git a/next/tp4/README.md b/next/tp4/README.md index 7ddb61a..8025611 100644 --- a/next/tp4/README.md +++ b/next/tp4/README.md @@ -49,4 +49,4 @@ These classes are not available in 2x and are reimplemented in Python following *getConstraintsJacobian* are reimplemented. ### Z axis -pin.ZAxis is not available in 2x and is reimplemented in Python. \ No newline at end of file +pin.ZAxis is not available in 2x and is reimplemented in Python. diff --git a/next/tp4/compatibility.py b/next/tp4/compatibility.py index df1b701..22cc9a8 100644 --- a/next/tp4/compatibility.py +++ b/next/tp4/compatibility.py @@ -1,25 +1,26 @@ -''' +""" This file collects a bunch of helpers to make the transition between pinocchio2.7/hppfcl2.4 and pinocchio3x/hppfcl3x (version 2.99 and future). -- pin.GeometryModel as reversed init API +- pin.GeometryModel as reversed init API - normals are opposite in hppfcl -''' +""" + +import warnings -import pinocchio as pin import hppfcl import numpy as np +import pinocchio as pin from numpy.linalg import norm -import warnings -P3X = pin.__version__.split('.')[1] == '99' -HPPFCL3X = hppfcl.__version__.split('.')[1] == '99' +P3X = pin.__version__.split(".")[1] == "99" +HPPFCL3X = hppfcl.__version__.split(".")[1] == "99" -P3X = [int(n) for n in pin.__version__.split('.') ] >= [2,99] -HPPFCL3X = [int(n) for n in hppfcl.__version__.split('.') ] >= [2,99] +P3X = [int(n) for n in pin.__version__.split(".")] >= [2, 99] +HPPFCL3X = [int(n) for n in hppfcl.__version__.split(".")] >= [2, 99] # ------------------------------------------------------------------------------- if not P3X: - pin.ZAxis = np.array([0,0,1.]) + pin.ZAxis = np.array([0, 0, 1.0]) # ------------------------------------------------------------------------------- # Monkey patch of computeDistances and computeCollisions to mimic p3x behavior @@ -28,114 +29,131 @@ pin._computeDistances = pin.computeDistances pin._computeCollisions = pin.computeCollisions - - def refineDistance_SphSph(g1,g2,oMg1,oMg2,res): - o1o2 = oMg2.translation-oMg1.translation + def refineDistance_SphSph(g1, g2, oMg1, oMg2, res): + o1o2 = oMg2.translation - oMg1.translation res.normal = o1o2 / norm(o1o2) - def refineDistance_SphPlane(g1,g2,oMg1,oMg2,res): - p1p2 = res.getNearestPoint2()-res.getNearestPoint1() - normal = oMg2.rotation@g2.geometry.n - assert( np.allclose(np.cross(p1p2,normal),0) ) + def refineDistance_SphPlane(g1, g2, oMg1, oMg2, res): + p1p2 = res.getNearestPoint2() - res.getNearestPoint1() + normal = oMg2.rotation @ g2.geometry.n + assert np.allclose(np.cross(p1p2, normal), 0) res.normal = -normal # res.normal = -normal if res.min_distance<0 else normal # res.normal = normal if np.dot(p1p2,normal)>=0 else -normal - def refineDistance_BoxPlane(g1,g2,oMg1,oMg2,res): + def refineDistance_BoxPlane(g1, g2, oMg1, oMg2, res): # This one is not working - p1p2 = res.getNearestPoint2()-res.getNearestPoint1() - normal = oMg2.rotation@g2.geometry.n - assert( np.allclose(np.cross(p1p2,normal),0) ) - #res.normal = -normal if d.min_distance<0 else normal #np.dot(p1p2,normal)>=0 else -normal + p1p2 = res.getNearestPoint2() - res.getNearestPoint1() + normal = oMg2.rotation @ g2.geometry.n + assert np.allclose(np.cross(p1p2, normal), 0) + # res.normal = -normal if d.min_distance<0 else normal #np.dot(p1p2,normal)>=0 else -normal res.normal = -normal - def computeDistances(model,data,geometry_model,geometry_data,q): - ''' + def computeDistances(model, data, geometry_model, geometry_data, q): + """ Mimic the behavior of computeDistances in pinocchio3x, by reversing the normals. - ''' - pin._computeDistances(model,data,geometry_model,geometry_data,q) - for ip,d in enumerate(geometry_data.distanceResults): + """ + pin._computeDistances(model, data, geometry_model, geometry_data, q) + for ip, d in enumerate(geometry_data.distanceResults): pair = geometry_model.collisionPairs[ip] - i1,i2 = pair.first,pair.second - g1,g2 = geometry_model.geometryObjects[i1],geometry_model.geometryObjects[i2] - sh1,sh2 = g1.geometry,g2.geometry - oMg1,oMg2 = geometry_data.oMg[i1],geometry_data.oMg[i2] + i1, i2 = pair.first, pair.second + g1, g2 = ( + geometry_model.geometryObjects[i1], + geometry_model.geometryObjects[i2], + ) + sh1, sh2 = g1.geometry, g2.geometry + oMg1, oMg2 = geometry_data.oMg[i1], geometry_data.oMg[i2] # Sphere Sphere - if isinstance(sh1,hppfcl.Sphere) and isinstance(sh2,hppfcl.Sphere): - refineDistance_SphSph(g1,g2,oMg1,oMg2,d) + if isinstance(sh1, hppfcl.Sphere) and isinstance(sh2, hppfcl.Sphere): + refineDistance_SphSph(g1, g2, oMg1, oMg2, d) # Box box - elif isinstance(sh1,hppfcl.Box) and isinstance(sh2,hppfcl.Box): - #refineDistance_BoxBox(g1,g2,oMg1,oMg2,d) - #if d.min_distance<1e-3: stop - print('Box-box collisions not working in P2X') - assert(False and 'Box-box collisions not working in P2X') + elif isinstance(sh1, hppfcl.Box) and isinstance(sh2, hppfcl.Box): + # refineDistance_BoxBox(g1,g2,oMg1,oMg2,d) + # if d.min_distance<1e-3: stop + print("Box-box collisions not working in P2X") + assert False and "Box-box collisions not working in P2X" # Sphere Plane - elif isinstance(sh1,hppfcl.Sphere) and isinstance(sh2,hppfcl.Halfspace): - refineDistance_SphPlane(g1,g2,oMg1,oMg2,d) - elif isinstance(sh1,hppfcl.Halfspace) and isinstance(sh2,hppfcl.Sphere): - refineDistance_SphPlane(g2,g1,oMg2,oMg1,d) + elif isinstance(sh1, hppfcl.Sphere) and isinstance(sh2, hppfcl.Halfspace): + refineDistance_SphPlane(g1, g2, oMg1, oMg2, d) + elif isinstance(sh1, hppfcl.Halfspace) and isinstance(sh2, hppfcl.Sphere): + refineDistance_SphPlane(g2, g1, oMg2, oMg1, d) # Box Plane - elif isinstance(sh1,hppfcl.Box) and isinstance(sh2,hppfcl.Halfspace): - refineDistance_BoxPlane(g1,g2,oMg1,oMg2,d) - elif isinstance(sh1,hppfcl.Halfspace) and isinstance(sh2,hppfcl.Box): - refineDistance_BoxPlane(g2,g1,oMg2,oMg1,d) + elif isinstance(sh1, hppfcl.Box) and isinstance(sh2, hppfcl.Halfspace): + refineDistance_BoxPlane(g1, g2, oMg1, oMg2, d) + elif isinstance(sh1, hppfcl.Halfspace) and isinstance(sh2, hppfcl.Box): + refineDistance_BoxPlane(g2, g1, oMg2, oMg1, d) # Misc else: if not np.any(np.isnan(d.normal)): - d.normal *= -1 + d.normal *= -1 # Check normal against witness direction, just to be sure witness = d.getNearestPoint2() - d.getNearestPoint1() w = np.linalg.norm(witness) - if w>1e-5: - if not np.allclose(witness/w,d.normal): - msg = f"Normal not aligned with witness segment (pair {ip} " \ + if w > 1e-5: + if not np.allclose(witness / w, d.normal): + msg = ( + f"Normal not aligned with witness segment (pair {ip} " + f"{type(sh1)}-{type(sh2)})" + ) warnings.warn(msg, category=UserWarning, stacklevel=2) else: # Poor patch, not working in penetration - print('# Poor patch, not working in penetration',ip,sh1,sh2) - msg = f"Setting normals from witness segment (pair {ip} " \ - + f"{type(sh1)}-{type(sh2)}) ### Poor patch, not working in penetration" + print("# Poor patch, not working in penetration", ip, sh1, sh2) + msg = ( + f"Setting normals from witness segment (pair {ip} " + + f"{type(sh1)}-{type(sh2)}) ### Poor patch, not working in penetration" + ) warnings.warn(msg, category=UserWarning, stacklevel=2) witness = d.getNearestPoint2() - d.getNearestPoint1() w = np.linalg.norm(witness) - assert(w>1e-5) - d.normal = witness/w + assert w > 1e-5 + d.normal = witness / w - def computeCollisions(model,data,geometry_model,geometry_data,q,stop_at_first_collision=False): - ''' + def computeCollisions( + model, data, geometry_model, geometry_data, q, stop_at_first_collision=False + ): + """ Mimic the behavior of pin.computeCollisions by relying on computeDistances which is more generic in p2x. BIG LIMITATIONS: only one single contact point can be detected - ''' + """ isInCollision = False - computeDistances(model,data,geometry_model,geometry_data,q) - for p,cr,c,d in zip(geometry_model.collisionPairs, - geometry_data.collisionRequests, - geometry_data.collisionResults, - geometry_data.distanceResults): + computeDistances(model, data, geometry_model, geometry_data, q) + for p, cr, c, d in zip( + geometry_model.collisionPairs, + geometry_data.collisionRequests, + geometry_data.collisionResults, + geometry_data.distanceResults, + ): c.clear() - id1,id2 = p.first,p.second - g1,g2 = geometry_model.geometryObjects[id1],geometry_model.geometryObjects[id2] + id1, id2 = p.first, p.second + g1, g2 = ( + geometry_model.geometryObjects[id1], + geometry_model.geometryObjects[id2], + ) # dist = np.dot(d.normal,d.getNearestPoint2()-d.getNearestPoint1()) dist = d.min_distance if dist < cr.security_margin: - contact = hppfcl.Contact(g1.geometry,g2.geometry, - d.b1,d.b2, - (d.getNearestPoint1()+d.getNearestPoint2())/2, - d.normal, - dist) + contact = hppfcl.Contact( + g1.geometry, + g2.geometry, + d.b1, + d.b2, + (d.getNearestPoint1() + d.getNearestPoint2()) / 2, + d.normal, + dist, + ) c.addContact(contact) isInCollision = True return isInCollision - computeDistances.__doc__ += '\n\nOriginal doc:\n' + pin._computeDistances.__doc__ - computeCollisions.__doc__ += '\n\nOriginal doc:\n' + pin._computeCollisions.__doc__ + computeDistances.__doc__ += "\n\nOriginal doc:\n" + pin._computeDistances.__doc__ + computeCollisions.__doc__ += "\n\nOriginal doc:\n" + pin._computeCollisions.__doc__ pin.computeDistances = computeDistances pin.computeCollisions = computeCollisions @@ -146,8 +164,8 @@ def computeCollisions(model,data,geometry_model,geometry_data,q,stop_at_first_co # ------------------------------------------------------------------------------- if not P3X: - from enum import Enum + class ContactType(Enum): CONTACT_3D = 3 CONTACT_6D = 6 @@ -156,16 +174,18 @@ class RigidConstraintData: pass class RigidConstraintModel: - def __init__(self, - contactType, - model: pin.Model, - joint1_id, - joint1_placement, - joint2_id, - joint2_placement, - referenceFrame): - assert(referenceFrame in pin.ReferenceFrame.values) - assert(contactType in [ ContactType.CONTACT_3D, ContactType.CONTACT_6D ]) + def __init__( + self, + contactType, + model: pin.Model, + joint1_id, + joint1_placement, + joint2_id, + joint2_placement, + referenceFrame, + ): + assert referenceFrame in pin.ReferenceFrame.values + assert contactType in [ContactType.CONTACT_3D, ContactType.CONTACT_6D] self.type = contactType self.joint1_id = joint1_id @@ -186,77 +206,90 @@ def createData(self): # ------------------------------------------------------------------------------- # Monkey patch of getConstraintsJacobian + def _getConstraintJacobian3d(model, data, contact_model, contact_data): - ''' + """ Returns the constraint Jacobian for 3d contact - ''' - assert(contact_model.type == pin.ContactType.CONTACT_3D) - assert(contact_model.reference_frame == pin.LOCAL) - c1_J_j1 = contact_model.joint1_placement.inverse().action[:3]@ \ - pin.getJointJacobian(model, data, contact_model.joint1_id, pin.LOCAL) - c2_J_j2 = contact_model.joint2_placement.inverse().action[:3]@ \ - pin.getJointJacobian(model, data, contact_model.joint2_id, pin.LOCAL) - c1Rc2 = contact_model.joint1_placement.rotation.T @ \ - data.oMi[contact_model.joint1_id].rotation.T @ \ - data.oMi[contact_model.joint2_id].rotation @ \ - contact_model.joint2_placement.rotation - + """ + assert contact_model.type == pin.ContactType.CONTACT_3D + assert contact_model.reference_frame == pin.LOCAL + c1_J_j1 = contact_model.joint1_placement.inverse().action[ + :3 + ] @ pin.getJointJacobian(model, data, contact_model.joint1_id, pin.LOCAL) + c2_J_j2 = contact_model.joint2_placement.inverse().action[ + :3 + ] @ pin.getJointJacobian(model, data, contact_model.joint2_id, pin.LOCAL) + c1Rc2 = ( + contact_model.joint1_placement.rotation.T + @ data.oMi[contact_model.joint1_id].rotation.T + @ data.oMi[contact_model.joint2_id].rotation + @ contact_model.joint2_placement.rotation + ) + J = c1_J_j1 J -= c1Rc2 @ c2_J_j2 - return(J) + return J + def _getConstraintJacobian6d(model, data, contact_model, contact_data): - ''' + """ Returns the constraint Jacobian - ''' - assert(contact_model.type == pin.ContactType.CONTACT_6D) - assert(contact_model.reference_frame == pin.LOCAL) - c1_J_j1 = contact_model.joint1_placement.inverse().action@ \ - pin.getJointJacobian(model, data, contact_model.joint1_id, pin.LOCAL) - c1_J_j2 = contact_model.joint1_placement.inverse().action@ \ - data.oMi[contact_model.joint1_id].inverse().action@ \ - data.oMi[contact_model.joint2_id].action@ \ - pin.getJointJacobian(model, data, contact_model.joint2_id, pin.LOCAL) + """ + assert contact_model.type == pin.ContactType.CONTACT_6D + assert contact_model.reference_frame == pin.LOCAL + c1_J_j1 = contact_model.joint1_placement.inverse().action @ pin.getJointJacobian( + model, data, contact_model.joint1_id, pin.LOCAL + ) + c1_J_j2 = ( + contact_model.joint1_placement.inverse().action + @ data.oMi[contact_model.joint1_id].inverse().action + @ data.oMi[contact_model.joint2_id].action + @ pin.getJointJacobian(model, data, contact_model.joint2_id, pin.LOCAL) + ) J = c1_J_j1 - c1_J_j2 - return(J) + return J + def _getConstraintJacobian(model, data, contact_model, contact_data): if contact_model.type == pin.ContactType.CONTACT_6D: - return _getConstraintJacobian6d(model,data,contact_model,contact_data) + return _getConstraintJacobian6d(model, data, contact_model, contact_data) elif contact_model.type == pin.ContactType.CONTACT_3D: - return _getConstraintJacobian3d(model,data,contact_model,contact_data) + return _getConstraintJacobian3d(model, data, contact_model, contact_data) else: - assert(False and "That's the two only possible contact types.") + assert False and "That's the two only possible contact types." + def _getConstraintsJacobian(model, data, constraint_models, constraint_datas): nc = len(constraint_models) - assert(len(constraint_datas)==nc) + assert len(constraint_datas) == nc Js = [] for cm, cd in zip(constraint_models, constraint_datas): Js.append(_getConstraintJacobian(model, data, cm, cd)) return np.vstack(Js) -if not P3X: +if not P3X: pin.getConstraintJacobian = _getConstraintJacobian pin.getConstraintsJacobian = _getConstraintsJacobian # ------------------------------------------------------------------------------- # Monkey patch of computeMinverse -def _computeMinverse(model:pin.Model, - data:pin.Data, - q: np.ndarray = None): - ''' + +def _computeMinverse(model: pin.Model, data: pin.Data, q: np.ndarray = None): + """ Monkey patch of computeMinverse. This is to allow a similar optional argument for q in p2x. Now you can write: pin.computeMinverse(model,data,None if P3X else q). - ''' - assert(q is not None or P3X) - if q is None: return pin._computeMinverse(model,data) - else: return pin._computeMinverse(model,data,q) + """ + assert q is not None or P3X + if q is None: + return pin._computeMinverse(model, data) + else: + return pin._computeMinverse(model, data, q) + pin._computeMinverse = pin.computeMinverse pin.computeMinverse = _computeMinverse diff --git a/next/tp4/create_rigid_contact_models_for_hppfcl.py b/next/tp4/create_rigid_contact_models_for_hppfcl.py index 8d72240..05cb202 100644 --- a/next/tp4/create_rigid_contact_models_for_hppfcl.py +++ b/next/tp4/create_rigid_contact_models_for_hppfcl.py @@ -1,21 +1,22 @@ -''' +""" Define the functions to create a set of pin.RigidConstraintModels from the results of the collision and distance functions. -''' +""" -import pinocchio as pin -import hppfcl import numpy as np -from tp4.compatibility import HPPFCL3X +import pinocchio as pin # Reference vector to decide how the contact frames must be selected: # -> with the normal to the contact along the z direction. CONTACT_TEMPLATE_NAME = "collision_{pairId}_{contactId}" -def _createOneConstraint(model,data,geom_model,geom_data,pairId,OC1,OC2,normal,contactId=0): - ''' - Atomic function to define a single 3D contact model from a collision or a distance + +def _createOneConstraint( + model, data, geom_model, geom_data, pairId, OC1, OC2, normal, contactId=0 +): + """ + Atomic function to define a single 3D contact model from a collision or a distance point. From the collision pair, get the joints ID and placements. @@ -29,24 +30,26 @@ def _createOneConstraint(model,data,geom_model,geom_data,pairId,OC1,OC2,normal,c When called from collision function, multiple witness pairs can occur per pair of collision body, in that case this field is used to make the name of each witness different. - ''' + """ # %jupyter_snippet frames pair = geom_model.collisionPairs[pairId] - gid1,gid2 = pair.first,pair.second + gid1, gid2 = pair.first, pair.second g1 = geom_model.geometryObjects[gid1] g2 = geom_model.geometryObjects[gid2] - jid1 = g1.parentJoint - jid2 = g2.parentJoint + jid1 = g1.parentJoint + jid2 = g2.parentJoint oMj1 = data.oMi[jid1] oMj2 = data.oMi[jid2] # Compute translation and rotation of the contact placements # If dist=0, both placements are identical (and should be somehow close # when dist is reasonibly small). - quat = pin.Quaternion.FromTwoVectors(pin.ZAxis,normal) # orientation of the contact frame wrt world - assert(np.isclose(quat.norm(),1)) - oMc1 = pin.SE3(quat.matrix(),OC1) # Placement of first contact frame in world - oMc2 = pin.SE3(quat.matrix(),OC2) # Placement of second contact frame in world + quat = pin.Quaternion.FromTwoVectors( + pin.ZAxis, normal + ) # orientation of the contact frame wrt world + assert np.isclose(quat.norm(), 1) + oMc1 = pin.SE3(quat.matrix(), OC1) # Placement of first contact frame in world + oMc2 = pin.SE3(quat.matrix(), OC2) # Placement of second contact frame in world # %end_jupyter_snippet # Finally, define the contact model for this point with Pinocchio structure @@ -54,42 +57,50 @@ def _createOneConstraint(model,data,geom_model,geom_data,pairId,OC1,OC2,normal,c contact_model = pin.RigidConstraintModel( pin.ContactType.CONTACT_3D, model, - jid1,oMj1.inverse()*oMc1, - jid2,oMj2.inverse()*oMc2, - pin.LOCAL) + jid1, + oMj1.inverse() * oMc1, + jid2, + oMj2.inverse() * oMc2, + pin.LOCAL, + ) # %end_jupyter_snippet - contact_model.name = CONTACT_TEMPLATE_NAME.format(pairId=pairId,contactId=contactId) + contact_model.name = CONTACT_TEMPLATE_NAME.format( + pairId=pairId, contactId=contactId + ) return contact_model -def createContactModelsFromCollisions(model,data,geom_model,geom_data): - ''' +def createContactModelsFromCollisions(model, data, geom_model, geom_data): + """ Create the contact models for each active collision in geom_data. It is supposed computeCollisions has already been called. For each collision, define a RigidConstraintModel with 2 reference placements: - one for each collision points (hopefully, they are close to each other) - centered at the witness point, and oriented with Z in the normal direction. Return a list of RigidConstraintModel. - ''' + """ contact_models = [] - for collId,r in enumerate(geom_data.collisionResults): - if r.numContacts()>0: + for collId, r in enumerate(geom_data.collisionResults): + if r.numContacts() > 0: for c in r.getContacts(): - OC1 = c.getNearestPoint1() # Position of first contact point in world - OC2 = c.getNearestPoint2() # Position of second contact point in world + OC1 = c.getNearestPoint1() # Position of first contact point in world + OC2 = c.getNearestPoint2() # Position of second contact point in world # In some simu solver, it might be prefered to have the contact point in between the collisions. # OC1=OC2=(OC1+OC2)/2 # In 2x, this is the default behavior (p1=p2=pos) - cm = _createOneConstraint(model,data,geom_model,geom_data,collId,OC1,OC2,c.normal) - + cm = _createOneConstraint( + model, data, geom_model, geom_data, collId, OC1, OC2, c.normal + ) + contact_models.append(cm) return contact_models -def createContactModelsFromDistances(model,data,geom_model,geom_data,threshold): - ''' + +def createContactModelsFromDistances(model, data, geom_model, geom_data, threshold): + """ Create the contact models for each distance in geom_data whose min_distance is below . It is supposed computeDistances has already been called. @@ -97,55 +108,64 @@ def createContactModelsFromDistances(model,data,geom_model,geom_data,threshold): - one for each witness points. - centered at the witness point, and oriented with Z in the normal direction. Return a list of RigidConstraintModel. - ''' + """ contact_models = [] - for collId,r in enumerate(geom_data.distanceResults): - if r.min_distance>threshold: continue + for collId, r in enumerate(geom_data.distanceResults): + if r.min_distance > threshold: + continue + + OC1 = r.getNearestPoint1() # Position of first contact point in world + OC2 = r.getNearestPoint2() # Position of second contact point in world - OC1 = r.getNearestPoint1() # Position of first contact point in world - OC2 = r.getNearestPoint2() # Position of second contact point in world - - cm = _createOneConstraint(model,data,geom_model,geom_data,collId,OC1,OC2,r.normal) + cm = _createOneConstraint( + model, data, geom_model, geom_data, collId, OC1, OC2, r.normal + ) contact_models.append(cm) return contact_models + if __name__ == "__main__": from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer from tp4.scenes import buildSceneThreeBodies - model,geom_model = buildSceneThreeBodies() + model, geom_model = buildSceneThreeBodies() data = model.createData() geom_data = geom_model.createData() # Start meshcat - viz = MeshcatVisualizer(model=model, collision_model=geom_model, - visual_model=geom_model) + viz = MeshcatVisualizer( + model=model, collision_model=geom_model, visual_model=geom_model + ) # Force the collision margin to a huge value. for r in geom_data.collisionRequests: r.security_margin = 10 q = pin.randomConfiguration(model) - q[2::7] += 1 # above the floor + q[2::7] += 1 # above the floor viz.display(q) # %jupyter_snippet example - pin.computeCollisions(model,data,geom_model,geom_data,q,False) - contact_models = createContactModelsFromCollisions(model,data,geom_model,geom_data) - contact_datas = [ cm.createData() for cm in contact_models ] - - pin.computeDistances(model,data,geom_model,geom_data,q) - contact_models = createContactModelsFromDistances(model,data,geom_model,geom_data, - threshold=10) # threshold in meter - contact_datas = [ cm.createData() for cm in contact_models ] + pin.computeCollisions(model, data, geom_model, geom_data, q, False) + contact_models = createContactModelsFromCollisions( + model, data, geom_model, geom_data + ) + contact_datas = [cm.createData() for cm in contact_models] + + pin.computeDistances(model, data, geom_model, geom_data, q) + contact_models = createContactModelsFromDistances( + model, data, geom_model, geom_data, threshold=10 + ) # threshold in meter + contact_datas = [cm.createData() for cm in contact_models] # %end_jupyter_snippet from display_witness import DisplayCollisionWitnessesInMeshcat + wdisp = DisplayCollisionWitnessesInMeshcat(viz) - pin.computeDistances(model,data,geom_model,geom_data,q) + pin.computeDistances(model, data, geom_model, geom_data, q) wdisp.displayDistances(geom_data) - assert(len(contact_models)==3) + assert len(contact_models) == 3 diff --git a/next/tp4/display_collision_patches.py b/next/tp4/display_collision_patches.py index 3ad0803..9cfe947 100644 --- a/next/tp4/display_collision_patches.py +++ b/next/tp4/display_collision_patches.py @@ -1,4 +1,4 @@ -''' +""" This file implements 2 helpers to simply visualize a set of contact models in Pinocchio. For each contact model, the vizualisation is composed of a pair of (red) disks placed parallel to the contact surfaces of the two collision objects. @@ -13,134 +13,157 @@ - updateVisualObjects: move the collision patches in the visual model. If the visualizer is provided, also automatically hide the unecessary patches. -''' +""" -import pinocchio as pin import hppfcl import numpy as np -import tp4.compatibility +import pinocchio as pin # ### HYPERPARAMETERS OF THE DISPLAY HELPERS COLPATCH_RADIUS = 0.05 COLPATCH_THICKNESS = 3e-3 -COLPATCH_COLOR = np.array([ 1,0,0,.5 ]) +COLPATCH_COLOR = np.array([1, 0, 0, 0.5]) COLPATCH_TEMPLATE_NAME = "collpatch_{ncolpatch}_{first_or_second}" COLPATCH_DEFAULT_PREALLOC = 10 # Create once for all the pin.GeometryObject used as colpatch visual. shape = hppfcl.Cylinder(COLPATCH_RADIUS, COLPATCH_THICKNESS) -geom = pin.GeometryObject( 'waitforit', 0,0, placement=pin.SE3.Identity(), - collision_geometry=shape) +geom = pin.GeometryObject( + "waitforit", 0, 0, placement=pin.SE3.Identity(), collision_geometry=shape +) geom.meshColor = COLPATCH_COLOR - -def _createVisualObjects(visual_model,ncolpatch,verbose=False): - ''' + + +def _createVisualObjects(visual_model, ncolpatch, verbose=False): + """ Create a single pair of patches. cammed by preallocateVisualObjects(). - ''' + """ if verbose: - print(f' --- Create a new visual patch #{ncolpatch}') - geom.name = COLPATCH_TEMPLATE_NAME.format(ncolpatch=ncolpatch,first_or_second='first') + print(f" --- Create a new visual patch #{ncolpatch}") + geom.name = COLPATCH_TEMPLATE_NAME.format( + ncolpatch=ncolpatch, first_or_second="first" + ) firstId = visual_model.addGeometryObject(geom) - geom.name = COLPATCH_TEMPLATE_NAME.format(ncolpatch=ncolpatch,first_or_second='second') + geom.name = COLPATCH_TEMPLATE_NAME.format( + ncolpatch=ncolpatch, first_or_second="second" + ) secondId = visual_model.addGeometryObject(geom) - return firstId,secondId + return firstId, secondId + def _whatIsMyVisualizer(visualizer): - ''' + """ Return a string defining the type of visualizer (none, meshcat, possibly gepettoviewer in the future). - ''' + """ if visualizer is None: return "none" try: import meshcat - if isinstance(visualizer.viewer,meshcat.visualizer.Visualizer): - return 'meshcat' + + if isinstance(visualizer.viewer, meshcat.visualizer.Visualizer): + return "meshcat" except ModuleNotFoundError: pass except AttributeError: pass -def _meshcat_hideColPatches(visual_model,visualizer,ntokeep,objectsAreSorted=True,verbose=False): - ''' + +def _meshcat_hideColPatches( + visual_model, visualizer, ntokeep, objectsAreSorted=True, verbose=False +): + """ There is no generic way to hide/show visuals for all viewers of Pinocchio. This is the specific instance for the meshcat viewer. If objectsAreSorted is true, then assume that the visual objects are sorted and in final position in the visual_model.geometryObjects list and make the seek-and-hide faster. - ''' - assert('viewer' in dir(visualizer)) - colpatchKey = COLPATCH_TEMPLATE_NAME.split('_')[0] + """ + assert "viewer" in dir(visualizer) + colpatchKey = COLPATCH_TEMPLATE_NAME.split("_")[0] if objectsAreSorted: - name0 = COLPATCH_TEMPLATE_NAME.format(ncolpatch=0,first_or_second='first') + name0 = COLPATCH_TEMPLATE_NAME.format(ncolpatch=0, first_or_second="first") idx0 = visual_model.getGeometryId(name0) - for ig,g in enumerate(visual_model.geometryObjects[idx0:]): - vis = ((ig//2)<=ntokeep) - adr = visualizer.getViewerNodeName(g,pin.VISUAL) - visualizer.viewer[adr].set_property('visible', vis) + for ig, g in enumerate(visual_model.geometryObjects[idx0:]): + vis = (ig // 2) <= ntokeep + adr = visualizer.getViewerNodeName(g, pin.VISUAL) + visualizer.viewer[adr].set_property("visible", vis) if verbose: - print(f'Make {ig//2} ({adr}) visible : {vis}') + print(f"Make {ig//2} ({adr}) visible : {vis}") else: for g in visual_model.geometryObjects: if colpatchKey in g.name: - ref = g.name.split('_')[1] + ref = g.name.split("_")[1] try: ref = int(ref) except ValueError: - print(f'Unexpected error with the colpatch name {g.name} ') - print(f'It was expected to follow the template {COLPATCH_TEMPLATE_NAME}') + print(f"Unexpected error with the colpatch name {g.name} ") + print( + f"It was expected to follow the template {COLPATCH_TEMPLATE_NAME}" + ) continue - vis = ref<=ntokeep - adr = visualizer.getViewerNodeName(g,pin.VISUAL) - visualizer.viewer[adr].set_property('visible', vis) + vis = ref <= ntokeep + adr = visualizer.getViewerNodeName(g, pin.VISUAL) + visualizer.viewer[adr].set_property("visible", vis) if verbose: - print(f'Make {ref} ({adr}) visible : {vis}') - -def preallocateVisualObjects(visual_model,number=COLPATCH_DEFAULT_PREALLOC,verbose=False): - ''' + print(f"Make {ref} ({adr}) visible : {vis}") + + +def preallocateVisualObjects( + visual_model, number=COLPATCH_DEFAULT_PREALLOC, verbose=False +): + """ Create the visual objects. This must be called before calling updateVisualObjects(). - ''' + """ for ic in range(number): - _createVisualObjects(visual_model,ic,verbose=verbose) + _createVisualObjects(visual_model, ic, verbose=verbose) + -def updateVisualObjects(model,data,contact_models,contact_datas,visual_model,visualizer=None): - ''' +def updateVisualObjects( + model, data, contact_models, contact_datas, visual_model, visualizer=None +): + """ Take the contact models list(pin.RigidConstraintModels) and update the placement of the visual objects in visual_model. In addition, it can hide the objects that are not useful, but this action is specific to the viewer and needs you to also pass the viewer (only for meshcat for now, but implementing it for Gepetto-viewer would be easy). - ''' + """ ic = -1 - for ic,[cmodel,cdata] in enumerate(zip(contact_models,contact_datas)): - - name_first = COLPATCH_TEMPLATE_NAME.format(ncolpatch=ic,first_or_second='first') + for ic, [cmodel, cdata] in enumerate(zip(contact_models, contact_datas)): + name_first = COLPATCH_TEMPLATE_NAME.format( + ncolpatch=ic, first_or_second="first" + ) gid_first = visual_model.getGeometryId(name_first) - name_second = COLPATCH_TEMPLATE_NAME.format(ncolpatch=ic,first_or_second='second') + name_second = COLPATCH_TEMPLATE_NAME.format( + ncolpatch=ic, first_or_second="second" + ) gid_second = visual_model.getGeometryId(name_second) - if gid_first==len(visual_model.geometryObjects): - print("There is not enough pre-loaded colpatch for displaying all collisions!") + if gid_first == len(visual_model.geometryObjects): + print( + "There is not enough pre-loaded colpatch for displaying all collisions!" + ) break - assert(gid_first and no more, either by creating the missing objects (ie all at the first iteration, but maybe less later) or by removing the extra objects (if more where allocated at the previous iteration). - ''' + """ if nobj < self.nwitnesses: # Remove extra objects - for idx_col in range(nobj,self.nwitnesses): - self.viz.delete(f'wit_{idx_col}_1') - self.viz.delete(f'wit_{idx_col}_2') - self.viz.delete(f'witseg_{idx_col}') + for idx_col in range(nobj, self.nwitnesses): + self.viz.delete(f"wit_{idx_col}_1") + self.viz.delete(f"wit_{idx_col}_2") + self.viz.delete(f"witseg_{idx_col}") else: - for idx_col in range(self.nwitnesses,nobj): - n = f'wit_{idx_col}_1' - self.viz.addSphere(n,self.RADIUS,'grey') - n = f'wit_{idx_col}_2' - self.viz.addSphere(n,self.RADIUS,'grey') - n = f'witseg_{idx_col}' - self.viz.addLine(n,self.A0B0[:3,0],self.A0B0[:3,1],'grey') - self.nwitnesses=nobj + for idx_col in range(self.nwitnesses, nobj): + n = f"wit_{idx_col}_1" + self.viz.addSphere(n, self.RADIUS, "grey") + n = f"wit_{idx_col}_2" + self.viz.addSphere(n, self.RADIUS, "grey") + n = f"witseg_{idx_col}" + self.viz.addLine(n, self.A0B0[:3, 0], self.A0B0[:3, 1], "grey") + self.nwitnesses = nobj - def _displayOnePair(self,idx_col,p1,p2,normal,dist=None): - ''' + def _displayOnePair(self, idx_col, p1, p2, normal, dist=None): + """ There is a trick for display the witness lines properly. In meshcat, lines are best defined with a pair of points. Yet meshcat does not give the interface to move these points, but only to apply a transform matrix M = [R p]. - We then arbitrarily defines the initial line with points a0,b0 (a0=0,0,0 + We then arbitrarily defines the initial line with points a0,b0 (a0=0,0,0 and b0=1,0,0 -- using a0=0 but any ||b0||=1 is acceptable). Then, if willing to display the line between p1 and p2 (with arbitrary coordinates @@ -66,45 +66,50 @@ def _displayOnePair(self,idx_col,p1,p2,normal,dist=None): We then chose p=p1 (using a0=0), and R=d.R' with d=||p1-p2|| the distance between the witnesses and R' the rotation matrix such that R'b0=n=(p2-p1)/d. - ''' - n = f'wit_{idx_col}_1' - self.viz.applyConfiguration(n,p1.tolist()+[1,0,0,0]) + """ + n = f"wit_{idx_col}_1" + self.viz.applyConfiguration(n, p1.tolist() + [1, 0, 0, 0]) - n = f'wit_{idx_col}_2' - self.viz.applyConfiguration(n,p2.tolist()+[1,0,0,0]) + n = f"wit_{idx_col}_2" + self.viz.applyConfiguration(n, p2.tolist() + [1, 0, 0, 0]) - n = f'witseg_{idx_col}' + n = f"witseg_{idx_col}" if dist is None: - dist = normal@(p2-p1) - quat = pin.Quaternion.FromTwoVectors(self.A0B0[:3,1],normal) - self.lineTransfo[:3,3] = p1 - self.lineTransfo[:3,:3] = quat.matrix()*dist - self.viz.applyConfiguration(n,self.lineTransfo) - - def displayCollisions(self,geom_data): - #assert(HPPFCL3X) # Only for 3x versions - self.resetMeshcatObjects(sum([ r.numContacts() for r in geom_data.collisionResults])) + dist = normal @ (p2 - p1) + quat = pin.Quaternion.FromTwoVectors(self.A0B0[:3, 1], normal) + self.lineTransfo[:3, 3] = p1 + self.lineTransfo[:3, :3] = quat.matrix() * dist + self.viz.applyConfiguration(n, self.lineTransfo) + + def displayCollisions(self, geom_data): + # assert(HPPFCL3X) # Only for 3x versions + self.resetMeshcatObjects( + sum([r.numContacts() for r in geom_data.collisionResults]) + ) idx_col = 0 - for collId,r in enumerate(geom_data.collisionResults): - if r.numContacts()==0: continue + for collId, r in enumerate(geom_data.collisionResults): + if r.numContacts() == 0: + continue for c in r.getContacts(): - assert(idx_col0: +for pairId, c in enumerate(geom_data.collisionResults): + if len(c.getContacts()) > 0: contact = c.getContact(0) - print([ n for n in dir(contact) if '__' not in n]) + print([n for n in dir(contact) if "__" not in n]) break # %end_jupyter_snippet diff --git a/next/tp4/example_proxqp.py b/next/tp4/example_proxqp.py index 616c561..2324a7b 100644 --- a/next/tp4/example_proxqp.py +++ b/next/tp4/example_proxqp.py @@ -1,8 +1,8 @@ -''' +""" Example of use of the QP solver ProxQP. See reference paper: Bambade et al, RSS 2022, https://www.roboticsproceedings.org/rss18/p040.pdf -The example randomly sample a QP problem with NX variables, NEQ equality constraints, +The example randomly sample a QP problem with NX variables, NEQ equality constraints, NIEQ inequality constraints and optional bounds (box constraints). Then, the solution is checked through various primal and dual tests. The thresholds of the tests could be more accurately adjusted, so some random instances @@ -19,29 +19,29 @@ Ae x = be (multipler denoted by y) bi[:,0] <= Ai x <= bi[:,1] (multiplier denoted by z) -''' +""" + +import random +import time -import proxsuite import numpy as np +import proxsuite from numpy.linalg import eig -import scipy.sparse as spa -import time -import random SEED = 1470 -SEED = int(time.time()%1*10000) -print('SEED = ',SEED) +SEED = int(time.time() % 1 * 10000) +print("SEED = ", SEED) np.random.seed(SEED) random.seed(SEED) # %jupyter_snippet param # ### TEST PARAMETERS -NX = 20 # x dimension (search space) -NEQ = 5 # number of equalities -NINEQ = 3 # Number of inequalities -WITH_BOUNDS = True # Additional bounds on x -VERBOSE = False # Do you want to see the result? -ACCURACY = 1e-6 # Threshold for solver stoping criteria and posterior checks +NX = 20 # x dimension (search space) +NEQ = 5 # number of equalities +NINEQ = 3 # Number of inequalities +WITH_BOUNDS = True # Additional bounds on x +VERBOSE = False # Do you want to see the result? +ACCURACY = 1e-6 # Threshold for solver stoping criteria and posterior checks # %end_jupyter_snippet # ### PROBLEM SETUP @@ -52,50 +52,62 @@ H = H @ H.T ### Make it positive symmetric g = np.random.rand(NX) -Ae = np.random.rand(NEQ, NX)*2-1 -be = np.random.rand(NEQ)*2-1 +Ae = np.random.rand(NEQ, NX) * 2 - 1 +be = np.random.rand(NEQ) * 2 - 1 -Ai = np.random.rand(NINEQ, NX)*2-1 -bi = np.sort(np.random.rand(NINEQ,2)*2-1,1) +Ai = np.random.rand(NINEQ, NX) * 2 - 1 +bi = np.sort(np.random.rand(NINEQ, 2) * 2 - 1, 1) for i in range(NINEQ): # Half inequalities are double bounds # One quarter are pure lower # One quarter are pure upper - r = random.randint(0,3) - if r==0: bi[i,0] = -1e20 - elif r==1: bi[i,1] = 1e20 + r = random.randint(0, 3) + if r == 0: + bi[i, 0] = -1e20 + elif r == 1: + bi[i, 1] = 1e20 -bounds = np.sort(np.random.rand(NX,2)*2-1,1)+[-1,1] +bounds = np.sort(np.random.rand(NX, 2) * 2 - 1, 1) + [-1, 1] for i in range(NX): # Half inequalities are double bounds # One quarter are pure lower # One quarter are pure upper - r = random.randint(0,3) - if r==0: bounds[i,0] = -1e20 - elif r==1: bounds[i,1] = 1e20 + r = random.randint(0, 3) + if r == 0: + bounds[i, 0] = -1e20 + elif r == 1: + bounds[i, 1] = 1e20 # %end_jupyter_snippet -assert(np.all(eig(H)[0] > 0)) -assert(np.all(bi[:,0] < bi[:,1])) -assert(np.all(bounds[:,0] < bounds[:,1])) +assert np.all(eig(H)[0] > 0) +assert np.all(bi[:, 0] < bi[:, 1]) +assert np.all(bounds[:, 0] < bounds[:, 1]) # ### SOLVER SETUP # %jupyter_snippet solve -#[x, cost, _, niter, lag, iact] = +# [x, cost, _, niter, lag, iact] = qp = proxsuite.proxqp.dense.QP(NX, NEQ, NINEQ, WITH_BOUNDS) -qp.settings.eps_abs = ACCURACY/1e3 -qp.init(H, g, Ae, be, Ai, bi[:,0], bi[:,1], - bounds[:,0] if WITH_BOUNDS else None, - bounds[:,1] if WITH_BOUNDS else None) +qp.settings.eps_abs = ACCURACY / 1e3 +qp.init( + H, + g, + Ae, + be, + Ai, + bi[:, 0], + bi[:, 1], + bounds[:, 0] if WITH_BOUNDS else None, + bounds[:, 1] if WITH_BOUNDS else None, +) qp.solve() # %end_jupyter_snippet # ### RESULT # %jupyter_snippet result -x,y,z = qp.results.x,qp.results.y,qp.results.z +x, y, z = qp.results.x, qp.results.y, qp.results.z if WITH_BOUNDS: - w = z[NINEQ:] # bounds - z = z[:NINEQ] # general inequalities + w = z[NINEQ:] # bounds + z = z[:NINEQ] # general inequalities cost = qp.results.info.objValue # %end_jupyter_snippet @@ -115,23 +127,25 @@ # Check primal KKT condition assert np.allclose(Ae @ x, be) -assert np.all(Ai @ x >= bi[:,0]-ACCURACY) -assert np.all(Ai @ x <= bi[:,1]+ACCURACY) +assert np.all(Ai @ x >= bi[:, 0] - ACCURACY) +assert np.all(Ai @ x <= bi[:, 1] + ACCURACY) # Check complementarity -assert abs(z@np.min([(Ai@x-bi[:,0]),(-Ai@x+bi[:,1])],0))ACCURACY*1e3: + if abs(z[i]) > ACCURACY * 1e3: # z negative corresponds to lower bound reach # z positive corresponds to upper bound reach - assert( (z[i]<0 and abs(Ai[i]@x-bi[i,0])0 and abs(Ai[i]@x-bi[i,1]) 0 and abs(Ai[i] @ x - bi[i, 1]) < ACCURACY + ) # Check complementarity of bound side for i in range(NX): - if abs(w[i])>ACCURACY*1e3: + if abs(w[i]) > ACCURACY * 1e3: # w negative corresponds to lower bound reach # w positive corresponds to upper bound reach - assert( (w[i]<0 and abs(x[i]-bounds[i,0])0 and abs(x[i]-bounds[i,1]) 0 and abs(x[i] - bounds[i, 1]) < ACCURACY + ) diff --git a/next/tp4/example_separate_objects.py b/next/tp4/example_separate_objects.py index f7de251..2864462 100644 --- a/next/tp4/example_separate_objects.py +++ b/next/tp4/example_separate_objects.py @@ -1,15 +1,17 @@ -import pinocchio as pin -import hppfcl +import matplotlib.pyplot as plt import numpy as np +import pinocchio as pin +from create_rigid_contact_models_for_hppfcl import ( + createContactModelsFromCollisions, + createContactModelsFromDistances, +) +from display_collision_patches import preallocateVisualObjects, updateVisualObjects +from scenes import buildScenePillsBox from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer from tp4.compatibility import P3X -from scenes import buildScenePillsBox,buildSceneThreeBodies -from display_collision_patches import preallocateVisualObjects,updateVisualObjects -from create_rigid_contact_models_for_hppfcl import createContactModelsFromCollisions,createContactModelsFromDistances -import matplotlib.pyplot as plt # Create scene with multiple objects -model,geom_model = buildScenePillsBox(nobj=30) +model, geom_model = buildScenePillsBox(nobj=30) # Create the corresponding data to the models data = model.createData() @@ -24,10 +26,11 @@ # Visualize ... # Add contact patches to the visual model (this is slow) visual_model = geom_model.copy() -preallocateVisualObjects(visual_model,100) -viz = MeshcatVisualizer(model=model, collision_model=geom_model, - visual_model=visual_model) -updateVisualObjects(model,data,[],[],visual_model,viz) +preallocateVisualObjects(visual_model, 100) +viz = MeshcatVisualizer( + model=model, collision_model=geom_model, visual_model=visual_model +) +updateVisualObjects(model, data, [], [], visual_model, viz) viz.display(q) # ### MAIN LOOP @@ -39,14 +42,14 @@ # %jupyter_snippet hyperparams # HYPER PARAMETERS OF THE PUSH STRATEGY -PUSH_FACTOR = .1 +PUSH_FACTOR = 0.1 EPSILON = 1e-1 NB_ITER = 100 # Compute the contact information based on distances or collisions? USE_DISTANCE = True # %end_jupyter_snippet # With P2X, only USE_DISTANCE=True is reasonible -assert(USE_DISTANCE or P3X) +assert USE_DISTANCE or P3X # %jupyter_snippet loop # Set minimal distance to be EPSILON @@ -59,27 +62,29 @@ # Iteratively push the colliding pairs ... for i in range(NB_ITER): - # We will compute a change of configuration dq. # 0 if no active pair. dq = np.zeros(model.nv) # Compute the collision at current configuration. if USE_DISTANCE: - pin.computeDistances(model,data,geom_model,geom_data,q) + pin.computeDistances(model, data, geom_model, geom_data, q) else: - pin.computeCollisions(model,data,geom_model,geom_data,q) + pin.computeCollisions(model, data, geom_model, geom_data, q) # From hppfcl contact information, build a pin.RigidContactModel if USE_DISTANCE: - contact_models = createContactModelsFromDistances(model,data,geom_model,geom_data,EPSILON) + contact_models = createContactModelsFromDistances( + model, data, geom_model, geom_data, EPSILON + ) else: - contact_models = createContactModelsFromCollisions(model,data,geom_model,geom_data) - contact_datas = [ cm.createData() for cm in contact_models ] + contact_models = createContactModelsFromCollisions( + model, data, geom_model, geom_data + ) + contact_datas = [cm.createData() for cm in contact_models] # For each detected contact ... - for cmodel,cdata in zip(contact_models,contact_datas): - + for cmodel, cdata in zip(contact_models, contact_datas): # Recover contact information jid1 = cmodel.joint1_id j1Mc1 = cmodel.joint1_placement @@ -87,23 +92,23 @@ j2Mc2 = cmodel.joint2_placement # Compute signed distance - oMc1 = cdata.oMc1 = data.oMi[jid1]*j1Mc1 - oMc2 = cdata.oMc2 = data.oMi[jid2]*j2Mc2 - dist = oMc1.actInv(oMc2.translation)[2]-EPSILON # signed distance - + oMc1 = cdata.oMc1 = data.oMi[jid1] * j1Mc1 + oMc2 = cdata.oMc2 = data.oMi[jid2] * j2Mc2 + dist = oMc1.actInv(oMc2.translation)[2] - EPSILON # signed distance + # Decide push velocity at contact point, in contact frames # We apply the same velocity proportional to the inverse of the signed # distance (exponential repel until EPSILON distance is reached) - c_v = PUSH_FACTOR*pin.Motion(np.array([0,0,dist]),np.zeros(3)) + c_v = PUSH_FACTOR * pin.Motion(np.array([0, 0, dist]), np.zeros(3)) # The velocities in joint space are expressed as spatial velocity # at the center of the respective joints. Displace c_v from F_c to F_j1/F_j2 # Displacement for body 1 j1_v = j1Mc1.act(c_v) - dq[model.idx_vs[jid1]:model.idx_vs[jid1]+6] += j1_v.vector + dq[model.idx_vs[jid1] : model.idx_vs[jid1] + 6] += j1_v.vector # Displacement for body 2 j2_v = j2Mc2.act(c_v) - dq[model.idx_vs[jid2]:model.idx_vs[jid2]+6] -= j2_v.vector + dq[model.idx_vs[jid2] : model.idx_vs[jid2] + 6] -= j2_v.vector # Log the distance in h_dist for future plot if cmodel.name not in h_dist: @@ -111,20 +116,22 @@ h_dist[cmodel.name][i] = dist # Finally, modify the current config q with the push dq - q = pin.integrate(model,q,dq) + q = pin.integrate(model, q, dq) # Display the current configuration if i % 10 == 0: # Meshcat is slow to display the patches, display once in a while - updateVisualObjects(model,data,contact_models,contact_datas,visual_model,viz) + updateVisualObjects( + model, data, contact_models, contact_datas, visual_model, viz + ) viz.display(q) # %end_jupyter_snippet # %jupyter_snippet plot # Plot the distances -for k,v in h_dist.items(): - h = plt.plot(v,label=k) +for k, v in h_dist.items(): + h = plt.plot(v, label=k) plt.legend() # %end_jupyter_snippet -print('Press plt.show() to display the plots.') +print("Press plt.show() to display the plots.") diff --git a/next/tp4/example_simu_frictionless.py b/next/tp4/example_simu_frictionless.py index 43900fd..1bf00db 100644 --- a/next/tp4/example_simu_frictionless.py +++ b/next/tp4/example_simu_frictionless.py @@ -1,26 +1,29 @@ -import pinocchio as pin -import hppfcl +import time + import numpy as np +import pinocchio as pin +from create_rigid_contact_models_for_hppfcl import createContactModelsFromCollisions +from display_collision_patches import preallocateVisualObjects, updateVisualObjects +from scenes import buildSceneCubes from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer from tp4.compatibility import P3X -from scenes import buildSceneCubes,addFloor -from display_collision_patches import preallocateVisualObjects,updateVisualObjects -from create_rigid_contact_models_for_hppfcl import createContactModelsFromCollisions -import matplotlib.pyplot as plt -import time -import proxsuite; QP = proxsuite.proxqp.dense.QP + +import proxsuite + +QP = proxsuite.proxqp.dense.QP # Parameters of the simulation -DURATION = 3. # duration of simulation -DT = 1e-3 # time step duration -DT_VISU = 1/100 -T = int(DURATION/DT) # number of time steps -WITH_CUBE_CORNERS = True # Use contacts only at cube corners +DURATION = 3.0 # duration of simulation +DT = 1e-3 # time step duration +DT_VISU = 1 / 100 +T = int(DURATION / DT) # number of time steps +WITH_CUBE_CORNERS = True # Use contacts only at cube corners PRIMAL_FORMULATION = True DUAL_FORMULATION = True -assert(PRIMAL_FORMULATION or DUAL_FORMULATION) +assert PRIMAL_FORMULATION or DUAL_FORMULATION # Random seed for simulation initialization -SEED = int(time.time()%1*1000); print('SEED = ',SEED) +SEED = int(time.time() % 1 * 1000) +print("SEED = ", SEED) SEED = 1 # ### RANDOM INIT @@ -31,7 +34,9 @@ # %jupyter_snippet init # ### SCENE # Create scene with multiple objects -model,geom_model = buildSceneCubes(1,with_floor=True,with_corner_collisions=True,with_cube_collisions=False) +model, geom_model = buildSceneCubes( + 1, with_floor=True, with_corner_collisions=True, with_cube_collisions=False +) # Create the corresponding data to the models data = model.createData() @@ -43,13 +48,14 @@ # ### VIZUALIZATION visual_model = geom_model.copy() -preallocateVisualObjects(visual_model,10) -viz = MeshcatVisualizer(model=model, collision_model=geom_model, - visual_model=visual_model) -updateVisualObjects(model,data,[],[],visual_model,viz) +preallocateVisualObjects(visual_model, 10) +viz = MeshcatVisualizer( + model=model, collision_model=geom_model, visual_model=visual_model +) +updateVisualObjects(model, data, [], [], visual_model, viz) # ### INIT MODEL STATE -q0 = model.referenceConfigurations['default'] +q0 = model.referenceConfigurations["default"] viz.display(q0) # %end_jupyter_snippet @@ -66,18 +72,19 @@ # ### MAIN LOOP # ### MAIN LOOP for t in range(T): - # Compute free dynamics tau = np.zeros(model.nv) pin.computeCollisions(model, data, geom_model, geom_data, q) vf = v + DT * pin.aba(model, data, q, v, tau) # Create contact models from collision - contact_models = createContactModelsFromCollisions(model,data,geom_model,geom_data) - contact_datas = [ cm.createData() for cm in contact_models ] - + contact_models = createContactModelsFromCollisions( + model, data, geom_model, geom_data + ) + contact_datas = [cm.createData() for cm in contact_models] + nc = len(contact_models) - if nc==0: + if nc == 0: # No collision, just integrate the free dynamics v = vf else: @@ -91,8 +98,10 @@ # normal component should be positive). # Select only normal components of contact # (frictionless slide on the tangent components, uncontrained) - J = -pin.getConstraintsJacobian(model, data, contact_models, contact_datas)[2::3,:] - assert(J.shape == (nc,model.nv)) + J = -pin.getConstraintsJacobian(model, data, contact_models, contact_datas)[ + 2::3, : + ] + assert J.shape == (nc, model.nv) # %end_jupyter_snippet if PRIMAL_FORMULATION: # ### PRIMAL_FORMULATION: @@ -100,8 +109,10 @@ # %jupyter_snippet primal # Solve the primal QP (search the velocity) # min_v .5 vMv - vfMv st Jv>=0 - qp1 = QP(model.nv,0,nc,False) - qp1.init(data.M,-data.M@vf,[],[],J,l=np.zeros(nc))#,u=np.ones(nc)*1e20) + qp1 = QP(model.nv, 0, nc, False) + qp1.init( + data.M, -data.M @ vf, [], [], J, l=np.zeros(nc) + ) # ,u=np.ones(nc)*1e20) qp1.settings.eps_abs = 1e-12 qp1.solve() @@ -111,12 +122,16 @@ forces = -qp1.results.z # Check the solution respects the physics - assert(np.all(forces>=-1e-6)) - assert(np.all(J@vnext>=-1e-6)) - assert(abs( forces@J@vnext)<1e-6) + assert np.all(forces >= -1e-6) + assert np.all(J @ vnext >= -1e-6) + assert abs(forces @ J @ vnext) < 1e-6 # Check the acceleration obtained from the forces - assert( np.allclose(pin.aba(model, data, q, v, tau + J.T @ forces/DT), - (vnext-v)/DT,rtol=1e-3,atol=1e-6)) + assert np.allclose( + pin.aba(model, data, q, v, tau + J.T @ forces / DT), + (vnext - v) / DT, + rtol=1e-3, + atol=1e-6, + ) # %end_jupyter_snippet if DUAL_FORMULATION: @@ -126,50 +141,56 @@ # min_f .5 f D f st f>=0, D f + J vf >= 0 # Compatibility issue: the 3rd argument is to enforce compatibility between # pinocchio 2x and 3x. Remove it if with P3X, or replace it by q if with P2X. - Minv = pin.computeMinverse(model,data,None if P3X else q) - delasus = J@Minv@J.T + Minv = pin.computeMinverse(model, data, None if P3X else q) + delasus = J @ Minv @ J.T - qp2 = QP(nc,0,nc,box_constraints=True) + qp2 = QP(nc, 0, nc, box_constraints=True) qp2.settings.eps_abs = 1e-12 # Both side of the box constraint must be given # otherwise the behavior of the solver is strange qp2.init( - H=delasus,g=np.zeros(nc), - C=delasus,l=-J@vf, - l_box=np.zeros(nc),u_box=np.ones(nc)*np.inf - ) + H=delasus, + g=np.zeros(nc), + C=delasus, + l=-J @ vf, + l_box=np.zeros(nc), + u_box=np.ones(nc) * np.inf, + ) qp2.solve() # Compute the contact acceleration from the forces forces = qp2.results.x - vnext = v + DT * pin.aba(model, data, q, v, tau + J.T @ forces/DT) + vnext = v + DT * pin.aba(model, data, q, v, tau + J.T @ forces / DT) # Check the solution respects the physics - assert(np.all(forces>=-1e-6)) - assert(np.all(J@vnext>=-1e-6)) - assert(abs( forces@J@vnext)<1e-6) + assert np.all(forces >= -1e-6) + assert np.all(J @ vnext >= -1e-6) + assert abs(forces @ J @ vnext) < 1e-6 # %end_jupyter_snippet if PRIMAL_FORMULATION and DUAL_FORMULATION: # %jupyter_snippet check # Check QP2 primal vs QP1 dual - assert(np.allclose(qp2.results.x,-qp1.results.z,rtol=1e-3,atol=1e-4)) + assert np.allclose(qp2.results.x, -qp1.results.z, rtol=1e-3, atol=1e-4) # Check QP2 constraint vs QP1 constraint - assert(np.allclose(delasus@qp2.results.x+J@vf, - J@qp1.results.x,rtol=1,atol=1e-5)) + assert np.allclose( + delasus @ qp2.results.x + J @ vf, J @ qp1.results.x, rtol=1, atol=1e-5 + ) # %end_jupyter_snippet v = vnext - + # Finally, integrate the valocity - q = pin.integrate(model , q, v*DT) + q = pin.integrate(model, q, v * DT) # Log hq += [q] hv += [v] # Visualize once in a while - if DT_VISU is not None and abs((t*DT) % DT_VISU)<=0.9*DT: - updateVisualObjects(model,data,contact_models,contact_datas,visual_model,viz) + if DT_VISU is not None and abs((t * DT) % DT_VISU) <= 0.9 * DT: + updateVisualObjects( + model, data, contact_models, contact_datas, visual_model, viz + ) viz.display(q) time.sleep(DT_VISU) diff --git a/next/tp4/generated/create_rigid_contact_models_for_hppfcl_frames b/next/tp4/generated/create_rigid_contact_models_for_hppfcl_frames index 0205842..ee122fe 100644 --- a/next/tp4/generated/create_rigid_contact_models_for_hppfcl_frames +++ b/next/tp4/generated/create_rigid_contact_models_for_hppfcl_frames @@ -2,8 +2,8 @@ pair = geom_model.collisionPairs[pairId] gid1,gid2 = pair.first,pair.second g1 = geom_model.geometryObjects[gid1] g2 = geom_model.geometryObjects[gid2] - jid1 = g1.parentJoint - jid2 = g2.parentJoint + jid1 = g1.parentJoint + jid2 = g2.parentJoint oMj1 = data.oMi[jid1] oMj2 = data.oMi[jid2] diff --git a/next/tp4/generated/example_separate_objects_loop b/next/tp4/generated/example_separate_objects_loop index 7ac5c38..c80d889 100644 --- a/next/tp4/generated/example_separate_objects_loop +++ b/next/tp4/generated/example_separate_objects_loop @@ -39,7 +39,7 @@ for i in range(NB_ITER): oMc1 = cdata.oMc1 = data.oMi[jid1]*j1Mc1 oMc2 = cdata.oMc2 = data.oMi[jid2]*j2Mc2 dist = oMc1.actInv(oMc2.translation)[2]-EPSILON # signed distance - + # Decide push velocity at contact point, in contact frames # We apply the same velocity proportional to the inverse of the signed # distance (exponential repel until EPSILON distance is reached) diff --git a/next/tp4/generated/example_simu_frictionless_loop b/next/tp4/generated/example_simu_frictionless_loop index 8939469..86fcd23 100644 --- a/next/tp4/generated/example_simu_frictionless_loop +++ b/next/tp4/generated/example_simu_frictionless_loop @@ -11,7 +11,7 @@ for t in range(T): # Create contact models from collision contact_models = createContactModelsFromCollisions(model,data,geom_model,geom_data) contact_datas = [ cm.createData() for cm in contact_models ] - + nc = len(contact_models) if nc==0: # No collision, just integrate the free dynamics diff --git a/next/tp4/robot_hand.py b/next/tp4/robot_hand.py index fdf45ad..5082407 100644 --- a/next/tp4/robot_hand.py +++ b/next/tp4/robot_hand.py @@ -61,7 +61,7 @@ def __init__(self): self.q0[2:-4] = np.pi / 6 self.q0[11:] = np.pi / 4 self.v0 = np.zeros(self.model.nv) - self.model.referenceConfigurations['default'] = self.q0.copy() + self.model.referenceConfigurations["default"] = self.q0.copy() self.collisionPairs = [] self.collision_model = self.gmodel @@ -446,8 +446,9 @@ def inertia(m, c): ) ) - self.model.lowerPositionLimit = np.ones(self.model.nq)*-2 - self.model.upperPositionLimit = np.ones(self.model.nq)*2 + self.model.lowerPositionLimit = np.ones(self.model.nq) * -2 + self.model.upperPositionLimit = np.ones(self.model.nq) * 2 + # # Prepare some patches to represent collision points. Yet unvisible. # if self.viewer is not None: # self.maxContact = 10 diff --git a/next/tp4/scenes.py b/next/tp4/scenes.py index 6ddfcb4..51266b9 100644 --- a/next/tp4/scenes.py +++ b/next/tp4/scenes.py @@ -1,15 +1,18 @@ -import pinocchio as pin -import hppfcl -import numpy as np import random -from supaero2024.hppfcl_utils import load_hppfcl_convex_from_stl_file -from tp4.compatibility import HPPFCL3X import unittest import warnings + +import hppfcl +import numpy as np +import pinocchio as pin +from supaero2024.hppfcl_utils import load_hppfcl_convex_from_stl_file from tp4.robot_hand import RobotHand -def buildScenePillsBox(nobj=30,wall_size=4.0,seed=0,no_walls=False,one_of_each=False): - ''' + +def buildScenePillsBox( + nobj=30, wall_size=4.0, seed=0, no_walls=False, one_of_each=False +): + """ Create pinocchio models (pin.Model and pin.GeometryModel) for a scene composed of a box (6 walls partly transparent) and objects of small size (capsules, ellipsoid, convex patatoid from STL). @@ -23,15 +26,15 @@ def buildScenePillsBox(nobj=30,wall_size=4.0,seed=0,no_walls=False,one_of_each=F - wall_size (float): The size of the walls of the box (default is 4.0). - seed (int): Seed value for random number generation (default is 0). - no_walls (bool): If True, no box is added (default is False). - - one_of_each (bool): If True, only one object of each type (ellipsoid, capsule, weird-shape) + - one_of_each (bool): If True, only one object of each type (ellipsoid, capsule, weird-shape) will be generated in turn (1,2,3,1,2,3,...) (default is False). Returns: - scene: A generated scene containing the specified number of pill-shaped objects within a box. - ''' + """ SEED = seed NOBJ = nobj - WORLD_SIZE = 0.45*wall_size + WORLD_SIZE = 0.45 * wall_size # Initialize the random generators pin.seed(SEED) @@ -48,71 +51,93 @@ def buildScenePillsBox(nobj=30,wall_size=4.0,seed=0,no_walls=False,one_of_each=F shapes = [ load_hppfcl_convex_from_stl_file("supaero2024/share/mesh.stl"), hppfcl.Ellipsoid(0.05, 0.15, 0.2), - hppfcl.Capsule(0.1, 0.2) + hppfcl.Capsule(0.1, 0.2), ] - for s in shapes: s.computeLocalAABB() + for s in shapes: + s.computeLocalAABB() # Joint limits - world_bounds = np.array([ WORLD_SIZE ] * 3 + [ np.inf ] * 4) - + world_bounds = np.array([WORLD_SIZE] * 3 + [np.inf] * 4) + if one_of_each: shapeSamples = [] - for _ in range((nobj+1)//len(shapes)): + for _ in range((nobj + 1) // len(shapes)): shapeSamples.extend(shapes) shapeSamples = shapeSamples[:nobj] else: - shapeSamples = random.sample(shapes*NOBJ,k=NOBJ) + shapeSamples = random.sample(shapes * NOBJ, k=NOBJ) for shape in shapeSamples: - jid = model.addJoint(0,pin.JointModelFreeFlyer(),pin.SE3.Identity(),'obj1', - min_config=-world_bounds,max_config=world_bounds, - max_velocity=np.ones(6),max_effort=np.ones(6)) + jid = model.addJoint( + 0, + pin.JointModelFreeFlyer(), + pin.SE3.Identity(), + "obj1", + min_config=-world_bounds, + max_config=world_bounds, + max_velocity=np.ones(6), + max_effort=np.ones(6), + ) color = np.random.rand(4) color[-1] = 1 # Place the object so that it is centered - Mcenter = pin.SE3(np.eye(3),-shape.computeCOM()) + Mcenter = pin.SE3(np.eye(3), -shape.computeCOM()) with warnings.catch_warnings(): warnings.simplefilter("ignore") - geom = pin.GeometryObject( f"{str(type(shape))[22:-2]}_{jid}", jid,jid, - placement=Mcenter,collision_geometry=shape) - + geom = pin.GeometryObject( + f"{str(type(shape))[22:-2]}_{jid}", + jid, + jid, + placement=Mcenter, + collision_geometry=shape, + ) + geom.meshColor = np.array(color) geom_model.addGeometryObject(geom) # Add inertia volum = shape.computeVolume() I = shape.computeMomentofInertia() - density = 700 # kg/m3 = wood - model.appendBodyToJoint(jid, - pin.Inertia(volum*density, np.zeros(3), I*density), - pin.SE3.Identity()) + density = 700 # kg/m3 = wood + model.appendBodyToJoint( + jid, + pin.Inertia(volum * density, np.zeros(3), I * density), + pin.SE3.Identity(), + ) # Add all pairs geom_model.addAllCollisionPairs() if not no_walls: - addBox(geom_model,wall_size) - - return model,geom_model + addBox(geom_model, wall_size) -def buildSceneThreeBodies(seed=0,size=1.0): - ''' + return model, geom_model + + +def buildSceneThreeBodies(seed=0, size=1.0): + """ Build a new scenes composed of 3 floating objects. This function is a proxy over the more general buildScenePillsBox. - + Parameters: - seed (int): Seed value for random number generation (default is 0). - size (float): The size of the scene (default is 1.0). Returns: - scene: A generated scene containing three pill-shape floating objects. - ''' - return buildScenePillsBox(nobj=3,no_walls=True,seed=seed, - one_of_each=True,wall_size=size) + """ + return buildScenePillsBox( + nobj=3, no_walls=True, seed=seed, one_of_each=True, wall_size=size + ) -def addFloor(geom_model,altitude=0,addCollisionPairs=True,color=np.array([0.9, 0.6, 0., .20])): - ''' +def addFloor( + geom_model, + altitude=0, + addCollisionPairs=True, + color=np.array([0.9, 0.6, 0.0, 0.20]), +): + """ Add an infinite horizontal plan to an existing scene. Parameters: @@ -122,8 +147,8 @@ def addFloor(geom_model,altitude=0,addCollisionPairs=True,color=np.array([0.9, 0 objects already in the scene (default is True). - color (numpy.ndarray): The color of the floor in RGBA format (default is np.array([0.9, 0.6, 0., .20])) with RGB-transparency syntax. - ''' - shape = hppfcl.Halfspace( np.array([0,0,1.]),0) + """ + shape = hppfcl.Halfspace(np.array([0, 0, 1.0]), 0) M = pin.SE3.Identity() M.translation[2] = altitude with warnings.catch_warnings(): @@ -133,14 +158,16 @@ def addFloor(geom_model,altitude=0,addCollisionPairs=True,color=np.array([0.9, 0 ifloor = geom_model.addGeometryObject(floor) # Collision pairs between all objects and the floor. - for ig,g in enumerate(geom_model.geometryObjects): - if g.name != 'floor': - geom_model.addCollisionPair(pin.CollisionPair(ig,ifloor)) + for ig, g in enumerate(geom_model.geometryObjects): + if g.name != "floor": + geom_model.addCollisionPair(pin.CollisionPair(ig, ifloor)) # geom_model.addCollisionPair(pin.CollisionPair(ifloor,ig)) -def addBox(geom_model,wall_size=4.0,color=np.array([1,1,1,0.2]),transparency=None): - ''' +def addBox( + geom_model, wall_size=4.0, color=np.array([1, 1, 1, 0.2]), transparency=None +): + """ Add a box composed of 6 transparent walls forming a cube. This box is typically though to come outside of a previously defined object set. @@ -148,87 +175,103 @@ def addBox(geom_model,wall_size=4.0,color=np.array([1,1,1,0.2]),transparency=Non - geom_model: The geometric model of the scene to which the box will be added. - wall_size (float): The size of each wall of the box (default is 4.0). - color (numpy.ndarray): The color of the box walls in RGBA format (default is np.array([1, 1, 1, 0.2])). - - transparency (float or None): The transparency of the box walls (default is None). If both color and + - transparency (float or None): The transparency of the box walls (default is None). If both color and transparency are set, the last component of the color will be ignored. - ''' + """ WALL_SIZE = wall_size - WALL_THICKNESS = WALL_SIZE*.05 - + WALL_THICKNESS = WALL_SIZE * 0.05 + wall_color = color.copy() - assert(len(wall_color)==4) + assert len(wall_color) == 4 if transparency is not None: wall_color[3] = transparency - + # X-axis M = pin.SE3.Identity() - M.translation = np.array([-WALL_SIZE-WALL_THICKNESS, 0., 0.])/2 + M.translation = np.array([-WALL_SIZE - WALL_THICKNESS, 0.0, 0.0]) / 2 shape = hppfcl.Box(WALL_THICKNESS, WALL_SIZE, WALL_SIZE) with warnings.catch_warnings(): warnings.simplefilter("ignore") - geom = pin.GeometryObject( f"wall_X-", 0,0, placement=M, collision_geometry=shape) + geom = pin.GeometryObject( + "wall_X-", 0, 0, placement=M, collision_geometry=shape + ) geom.meshColor = np.array(wall_color) geom_model.addGeometryObject(geom) - M = pin.SE3.Identity() - M.translation = np.array([WALL_SIZE, 0., 0.])/2 + M.translation = np.array([WALL_SIZE, 0.0, 0.0]) / 2 shape = hppfcl.Box(WALL_THICKNESS, WALL_SIZE, WALL_SIZE) with warnings.catch_warnings(): warnings.simplefilter("ignore") - geom = pin.GeometryObject( f"wall_X+", 0,0, placement=M, collision_geometry=shape) + geom = pin.GeometryObject( + "wall_X+", 0, 0, placement=M, collision_geometry=shape + ) geom.meshColor = np.array(wall_color) geom_model.addGeometryObject(geom) # Y-axis M = pin.SE3.Identity() - M.translation = np.array([0., -WALL_SIZE, 0.])/2 + M.translation = np.array([0.0, -WALL_SIZE, 0.0]) / 2 shape = hppfcl.Box(WALL_SIZE, WALL_THICKNESS, WALL_SIZE) with warnings.catch_warnings(): warnings.simplefilter("ignore") - geom = pin.GeometryObject( f"wall_Y-", 0,0, placement=M, collision_geometry=shape) + geom = pin.GeometryObject( + "wall_Y-", 0, 0, placement=M, collision_geometry=shape + ) geom.meshColor = np.array(wall_color) geom_model.addGeometryObject(geom) M = pin.SE3.Identity() - M.translation = np.array([0., WALL_SIZE, 0.])/2 + M.translation = np.array([0.0, WALL_SIZE, 0.0]) / 2 shape = hppfcl.Box(WALL_SIZE, WALL_THICKNESS, WALL_SIZE) with warnings.catch_warnings(): warnings.simplefilter("ignore") - geom = pin.GeometryObject( f"wall_Y+", 0,0, placement=M, collision_geometry=shape) + geom = pin.GeometryObject( + "wall_Y+", 0, 0, placement=M, collision_geometry=shape + ) geom.meshColor = np.array(wall_color) geom_model.addGeometryObject(geom) # Z-axis M = pin.SE3.Identity() - M.translation = np.array([0., 0., -WALL_SIZE])/2 + M.translation = np.array([0.0, 0.0, -WALL_SIZE]) / 2 shape = hppfcl.Box(WALL_SIZE, WALL_SIZE, WALL_THICKNESS) with warnings.catch_warnings(): warnings.simplefilter("ignore") - geom = pin.GeometryObject( f"wall_Z-", 0,0, placement=M, collision_geometry=shape) + geom = pin.GeometryObject( + "wall_Z-", 0, 0, placement=M, collision_geometry=shape + ) geom.meshColor = np.array(wall_color) geom_model.addGeometryObject(geom) M = pin.SE3.Identity() - M.translation = np.array([0., 0., WALL_SIZE])/2 + M.translation = np.array([0.0, 0.0, WALL_SIZE]) / 2 shape = hppfcl.Box(WALL_SIZE, WALL_SIZE, WALL_THICKNESS) with warnings.catch_warnings(): warnings.simplefilter("ignore") - geom = pin.GeometryObject( f"wall_Z+", 0,0, placement=M, collision_geometry=shape) + geom = pin.GeometryObject( + "wall_Z+", 0, 0, placement=M, collision_geometry=shape + ) geom.meshColor = np.array(wall_color) geom_model.addGeometryObject(geom) # Remove pairs between walls - for ig1,g1 in enumerate(geom_model.geometryObjects): - for ig2,g2 in enumerate(geom_model.geometryObjects): + for ig1, g1 in enumerate(geom_model.geometryObjects): + for ig2, g2 in enumerate(geom_model.geometryObjects): # a^b is XOR ( (a and not b) or (b and not a) ) - if ig1 < ig2 and ( ('wall' in g1.name) ^ ('wall' in g2.name) ): - geom_model.addCollisionPair(pin.CollisionPair(ig1,ig2)) - - -def buildSceneCubes(number_of_cubes,sizes=0.2, masses=1.0, - with_corner_collisions = True, with_cube_collisions = False, - with_floor=False, - corner_insider_the_cube = True): + if ig1 < ig2 and (("wall" in g1.name) ^ ("wall" in g2.name)): + geom_model.addCollisionPair(pin.CollisionPair(ig1, ig2)) + + +def buildSceneCubes( + number_of_cubes, + sizes=0.2, + masses=1.0, + with_corner_collisions=True, + with_cube_collisions=False, + with_floor=False, + corner_insider_the_cube=True, +): """Creates the pinocchio pin.Model and pin.GeometryModel of a scene containing cubes. The number of cubes is defines by the length of the two lists in argument. @@ -260,240 +303,289 @@ def buildSceneCubes(number_of_cubes,sizes=0.2, masses=1.0, """ N = number_of_cubes - if np.isscalar(sizes): sizes = [ sizes ]*N - if np.isscalar(masses): masses = [ masses ]*N - assert(len(sizes) == len(masses) == N) + if np.isscalar(sizes): + sizes = [sizes] * N + if np.isscalar(masses): + masses = [masses] * N + assert len(sizes) == len(masses) == N - BALL_FACTOR_SIZE = 1/50 + BALL_FACTOR_SIZE = 1 / 50 SPHERE_TEMPLATE_NAME = "cube_corner:{n_cube}:{n_sphere}" CUBE_TEMPLATE_NAME = "cube:{n_cube}" CUBE_COLOR = np.array([0.0, 0.0, 1.0, 0.6]) SPHERE_COLOR = np.array([1.0, 0.2, 0.2, 1.0]) - WORLD_BOUNDS = np.array([ max(sizes)*5 ] * 3 + [ np.inf ] * 4) + WORLD_BOUNDS = np.array([max(sizes) * 5] * 3 + [np.inf] * 4) model = pin.Model() geom_model = pin.GeometryModel() - ball_ids = [] - cube_joints = [] - for n_cube,(size,mass) in enumerate(zip(sizes,masses)): + for n_cube, (size, mass) in enumerate(zip(sizes, masses)): # to get random init above the floor low = -WORLD_BOUNDS - low[2] = size*np.sqrt(3)/2 + low[2] = size * np.sqrt(3) / 2 jointCube = model.addJoint( - 0, pin.JointModelFreeFlyer(), pin.SE3.Identity(), "joint1_" + str(n_cube), - min_config=low,max_config=WORLD_BOUNDS, - max_velocity=np.ones(6),max_effort=np.ones(6) + 0, + pin.JointModelFreeFlyer(), + pin.SE3.Identity(), + "joint1_" + str(n_cube), + min_config=low, + max_config=WORLD_BOUNDS, + max_velocity=np.ones(6), + max_effort=np.ones(6), + ) + model.appendBodyToJoint( + jointCube, pin.Inertia.FromBox(mass, size, size, size), pin.SE3.Identity() ) - model.appendBodyToJoint(jointCube, - pin.Inertia.FromBox(mass, size,size,size), - pin.SE3.Identity()) # Add cube visual - shape = hppfcl.Box(size,size,size) + shape = hppfcl.Box(size, size, size) with warnings.catch_warnings(): warnings.simplefilter("ignore") geom_box = pin.GeometryObject( - CUBE_TEMPLATE_NAME.format(n_cube=n_cube), jointCube, jointCube, - placement=pin.SE3.Identity(), collision_geometry=shape + CUBE_TEMPLATE_NAME.format(n_cube=n_cube), + jointCube, + jointCube, + placement=pin.SE3.Identity(), + collision_geometry=shape, ) geom_box.meshColor = CUBE_COLOR - box_id = geom_model.addGeometryObject(geom_box) # only for visualisation + geom_model.addGeometryObject(geom_box) # only for visualisation # Add corner collisions # For each corner at +size/2 or -size/2 for x y and z, add a small sphere. - ballSize = size*BALL_FACTOR_SIZE + ballSize = size * BALL_FACTOR_SIZE shape = hppfcl.Sphere(ballSize) M = pin.SE3.Identity() # TODO: Order x,y,z temporarily chosen to mimic the initial code. # Could be reset to a more classical order to avoid confusion later. - corners = [ np.array([x,y,z])*(size/2 - (ballSize if corner_insider_the_cube else 0)) - for x in [1,-1] for z in [-1,1] for y in [-1,1] ] - for n_sphere,trans in enumerate(corners): + corners = [ + np.array([x, y, z]) + * (size / 2 - (ballSize if corner_insider_the_cube else 0)) + for x in [1, -1] + for z in [-1, 1] + for y in [-1, 1] + ] + for n_sphere, trans in enumerate(corners): M.translation = trans - name = SPHERE_TEMPLATE_NAME.format(n_cube=n_cube,n_sphere=n_sphere) + name = SPHERE_TEMPLATE_NAME.format(n_cube=n_cube, n_sphere=n_sphere) with warnings.catch_warnings(): warnings.simplefilter("ignore") - geom_ball1 = pin.GeometryObject(name, jointCube, jointCube, placement=M, - collision_geometry=shape) + geom_ball1 = pin.GeometryObject( + name, jointCube, jointCube, placement=M, collision_geometry=shape + ) geom_ball1.meshColor = SPHERE_COLOR - ball1_id = geom_model.addGeometryObject(geom_ball1) - + geom_model.addGeometryObject(geom_ball1) # Collision pairs if with_corner_collisions: # Add collisions between corners of different cubes - assert('cube_corner:' in SPHERE_TEMPLATE_NAME) - for ig1,g1 in enumerate(geom_model.geometryObjects): - for ig2,g2 in enumerate(geom_model.geometryObjects): - if not ig110: + pin.computeCollisions(model, data, geom_model, geom_data, q0) + if sum([len(c.getContacts()) for c in geom_data.collisionResults]) > 10: break - print(sum([ len(c.getContacts()) for c in geom_data.collisionResults ])) + print(sum([len(c.getContacts()) for c in geom_data.collisionResults])) # %end_jupyter_snippet q = pin.randomConfiguration(model) viz.display(q) - for p in geom_model.collisionPairs: - i,j=p.first,p.second - print(geom_model.geometryObjects[i].name,geom_model.geometryObjects[j].name) + i, j = p.first, p.second + print(geom_model.geometryObjects[i].name, geom_model.geometryObjects[j].name) diff --git a/next/tp4/test_compatibility.py b/next/tp4/test_compatibility.py index 2342238..b5ed945 100644 --- a/next/tp4/test_compatibility.py +++ b/next/tp4/test_compatibility.py @@ -1,47 +1,62 @@ -from compatibility import P3X import unittest -import pinocchio as pin -import hppfcl -import numpy as np -from numpy.linalg import norm - +import numpy as np +import pinocchio as pin +from compatibility import ( + P3X, + _getConstraintJacobian3d, + _getConstraintJacobian6d, + _getConstraintsJacobian, +) ### TEST ZONE ############################################################ ### This last part is to automatically validate the versions of this example. -from compatibility import _getConstraintJacobian6d,_getConstraintJacobian3d,_getConstraintsJacobian class TestJconstraint(unittest.TestCase): def test_j6d(self): pass + from tp4.scenes import buildSceneThreeBodies -model,gmodel = buildSceneThreeBodies() + +model, gmodel = buildSceneThreeBodies() data = model.createData() -cmodel6 = pin.RigidConstraintModel(pin.ContactType.CONTACT_6D,model, - 1,pin.SE3.Random(), - 2,pin.SE3.Random(),pin.LOCAL) +cmodel6 = pin.RigidConstraintModel( + pin.ContactType.CONTACT_6D, + model, + 1, + pin.SE3.Random(), + 2, + pin.SE3.Random(), + pin.LOCAL, +) cdata6 = cmodel6.createData() q = pin.randomConfiguration(model) -pin.computeJointJacobians(model,data,q) -Ji = _getConstraintJacobian6d(model,data,cmodel6,cdata6) +pin.computeJointJacobians(model, data, q) +Ji = _getConstraintJacobian6d(model, data, cmodel6, cdata6) if P3X: - Je = pin.getConstraintJacobian(model,data,cmodel6,cdata6) - assert( np.allclose(Je,Ji) ) - -cmodel3 = pin.RigidConstraintModel(pin.ContactType.CONTACT_3D,model, - 1,pin.SE3.Random(), - 2,pin.SE3.Random(),pin.LOCAL) + Je = pin.getConstraintJacobian(model, data, cmodel6, cdata6) + assert np.allclose(Je, Ji) + +cmodel3 = pin.RigidConstraintModel( + pin.ContactType.CONTACT_3D, + model, + 1, + pin.SE3.Random(), + 2, + pin.SE3.Random(), + pin.LOCAL, +) cdata3 = cmodel3.createData() -Ji = _getConstraintJacobian3d(model,data,cmodel3,cdata3) +Ji = _getConstraintJacobian3d(model, data, cmodel3, cdata3) if P3X: - Je = pin.getConstraintJacobian(model,data,cmodel3,cdata3) - assert( np.allclose(Je,Ji) ) + Je = pin.getConstraintJacobian(model, data, cmodel3, cdata3) + assert np.allclose(Je, Ji) -Ji = _getConstraintsJacobian(model,data,[cmodel3,cmodel6],[cdata3,cdata6]) +Ji = _getConstraintsJacobian(model, data, [cmodel3, cmodel6], [cdata3, cdata6]) if P3X: - Je = pin.getConstraintsJacobian(model,data,[cmodel3,cmodel6],[cdata3,cdata6]) - assert( np.allclose(Je,Ji) ) + Je = pin.getConstraintsJacobian(model, data, [cmodel3, cmodel6], [cdata3, cdata6]) + assert np.allclose(Je, Ji) ### TEST ZONE ############################################################ diff --git a/next/tp5/bicopter_toward_origin.py b/next/tp5/bicopter_toward_origin.py index 986a444..a9e6df4 100644 --- a/next/tp5/bicopter_toward_origin.py +++ b/next/tp5/bicopter_toward_origin.py @@ -1,18 +1,19 @@ -import matplotlib.pyplot as plt -import numpy as np -import crocoddyl import unittest -from tp5.bicopter_utils import plotBicopterSolution,ViewerBicopter + +import crocoddyl +import numpy as np +from tp5.bicopter_utils import ViewerBicopter, plotBicopterSolution # %jupyter_snippet hyperparams ### HYPER PARAMS: horizon and initial state timeStep = 0.01 -x0 = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0]) +x0 = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0]) T = 50 # %end_jupyter_snippet ### MODEL DEFINITION + # Definition of the differential action model for the bicopter # The state x=(q,v) is the concatenation of position and velocity of the copter # (with q=(x1,x2,theta), x1 the horizontal position, x2 the vertical position and @@ -28,71 +29,84 @@ # and approximate Hessian (with Gauss H=J'J). # %jupyter_snippet dam_header class DifferentialActionModelBicopter(crocoddyl.DifferentialActionModelAbstract): - def __init__(self): - ''' - Init on top of the DAM class. + """ + Init on top of the DAM class. Mostly set up the hyperparameters of this model (mass, length, cost, etc). ng=0 says we have 0 constraints. - ''' + """ crocoddyl.DifferentialActionModelAbstract.__init__( self, crocoddyl.StateVector(6), nu=2, nr=12, ng=0 ) self.unone = np.zeros(self.nu) - self.span = .2 - self.mass = 2. + self.span = 0.2 + self.mass = 2.0 self.g = 10 - self.inertia = self.mass*self.span**2 + self.inertia = self.mass * self.span**2 self.costWeights = [ - 0.1, # x - 0.1, # z - .10, # s - .10, # c - 0.001, # vx - 0.001, # vz - 0.001, # w - 0.0, # fr - 0.0, # fl - 0.001,0.001,0.001, # a + 0.1, # x + 0.1, # z + 0.10, # s + 0.10, # c + 0.001, # vx + 0.001, # vz + 0.001, # w + 0.0, # fr + 0.0, # fl + 0.001, + 0.001, + 0.001, # a ] # sin, 1-cos, x, xdot, thdot, f def calc(self, data, x, u=None): if u is None: u = model.unone # Getting the state and control variables - x1,x2,th,v1,v2,w = x - fr,fl = u + x1, x2, th, v1, v2, w = x + fr, fl = u -# %end_jupyter_snippet + # %end_jupyter_snippet # Shortname for system parameters - mass,span,g,inertia=self.mass,self.span,self.g,self.inertia + mass, span, g, inertia = self.mass, self.span, self.g, self.inertia s, c = np.sin(th), np.cos(th) # Defining the equation of motions # Force (fx,fz,tauy) in local frame - loc_f = np.array([0, fr+fl, (fl-fr)*span]) + loc_f = np.array([0, fr + fl, (fl - fr) * span]) # Acceleration (x,z,th) in world frame - data.xout = np.array([ - -loc_f[1]*s/mass, - loc_f[1]*c/mass - g, - loc_f[2]/inertia - ]) + data.xout = np.array( + [-loc_f[1] * s / mass, loc_f[1] * c / mass - g, loc_f[2] / inertia] + ) # Computing the cost residual and value - data.r = self.costWeights * np.array([x1, x2, s, 1 - c, - v1, v2, w, - fr, fl, - data.xout[0], data.xout[1], data.xout[2] ]) - data.cost = 0.5 * sum(data.r ** 2) + data.r = self.costWeights * np.array( + [ + x1, + x2, + s, + 1 - c, + v1, + v2, + w, + fr, + fl, + data.xout[0], + data.xout[1], + data.xout[2], + ] + ) + data.cost = 0.5 * sum(data.r**2) -# %jupyter_snippet dam_calcdiff_template + # %jupyter_snippet dam_calcdiff_template def calcDiff(self, data, x, u=None): # Advance user might implement the derivatives. Here # we will rely on finite differences. pass + + # %end_jupyter_snippet # %jupyter_snippet dam @@ -104,8 +118,8 @@ def calcDiff(self, data, x, u=None): # Create a local DAM data for testing the implementation dad = dam.createData() x = dam.state.rand() -u = np.array([12,8]) -dam.calc(dad,x,u) +u = np.array([12, 8]) +dam.calc(dad, x, u) # %end_jupyter_snippet # %jupyter_snippet dam_nd @@ -126,13 +140,13 @@ def calcDiff(self, data, x, u=None): terminalDamND = crocoddyl.DifferentialActionModelNumDiff(terminalDam, True) terminalIam = crocoddyl.IntegratedActionModelEuler(terminalDamND) -terminalDam.costWeights[0] = 100 # horizontal position -terminalDam.costWeights[1] = 100 # vertical position -terminalDam.costWeights[2] = 100.0 # angle sin (first order) -terminalDam.costWeights[3] = 100.0 # angle cos (second order) -terminalDam.costWeights[4] = 100 # horizontal velocity -terminalDam.costWeights[5] = 100 # vertical velocity -terminalDam.costWeights[6] = 100 # angular velocity +terminalDam.costWeights[0] = 100 # horizontal position +terminalDam.costWeights[1] = 100 # vertical position +terminalDam.costWeights[2] = 100.0 # angle sin (first order) +terminalDam.costWeights[3] = 100.0 # angle cos (second order) +terminalDam.costWeights[4] = 100 # horizontal velocity +terminalDam.costWeights[5] = 100 # vertical velocity +terminalDam.costWeights[6] = 100 # angular velocity # %end_jupyter_snippet ### PROBLEM DEFINITION @@ -148,10 +162,10 @@ def calcDiff(self, data, x, u=None): ### SOLVE THE PROBLEM done = ddp.solve([], [], 300) -assert(done) +assert done # %end_jupyter_snippet -### PLOT +### PLOT # %jupyter_snippet plot log = ddp.getCallbacks()[0] @@ -170,23 +184,24 @@ def calcDiff(self, data, x, u=None): plotBicopterSolution(list(ddp.xs)[::3]) # %end_jupyter_snippet -print('Type plt.show() to display the result.') +print("Type plt.show() to display the result.") # %jupyter_snippet viz # Animate the solution in meshcat viz = ViewerBicopter() -viz.displayTrajectory(ddp.xs,timeStep) +viz.displayTrajectory(ddp.xs, timeStep) # %end_jupyter_snippet + ### TEST ZONE ############################################################ ### This last part is to automatically validate the versions of this example. class LocalTest(unittest.TestCase): def test_logs(self): print(self.__class__.__name__) - self.assertTrue( len(ddp.xs) == len(ddp.us)+1 ) - self.assertTrue( np.allclose(ddp.xs[0],ddp.problem.x0) ) - self.assertTrue( ddp.stop<1e-6 ) - + self.assertTrue(len(ddp.xs) == len(ddp.us) + 1) + self.assertTrue(np.allclose(ddp.xs[0], ddp.problem.x0)) + self.assertTrue(ddp.stop < 1e-6) + + if __name__ == "__main__": LocalTest().test_logs() - diff --git a/next/tp5/bicopter_toward_origin_with_constraints.py b/next/tp5/bicopter_toward_origin_with_constraints.py index f2b6c78..aeceab0 100644 --- a/next/tp5/bicopter_toward_origin_with_constraints.py +++ b/next/tp5/bicopter_toward_origin_with_constraints.py @@ -1,17 +1,16 @@ -import matplotlib.pyplot as plt -import numpy as np import crocoddyl -import unittest -from tp5.bicopter_utils import plotBicopterSolution,ViewerBicopter import mim_solvers +import numpy as np +from tp5.bicopter_utils import ViewerBicopter, plotBicopterSolution ### HYPER PARAMS: horizon and initial state timeStep = 0.01 -x0 = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0]) +x0 = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0]) T = 100 ### MODEL DEFINITION + # Definition of the differential action model for the bicopter # The state x=(q,v) is the concatenation of position and velocity of the copter # (with q=(x1,x2,theta), x1 the horizontal position, x2 the vertical position and @@ -27,62 +26,73 @@ # and approximate Hessian (with Gauss H=J'J). # %jupyter_snippet dam class DifferentialActionModelBicopter(crocoddyl.DifferentialActionModelAbstract): - def __init__(self): - ''' - Init on top of the DAM class. + """ + Init on top of the DAM class. Mostly set up the hyperparameters of this model (mass, length, cost, etc). - ''' + """ crocoddyl.DifferentialActionModelAbstract.__init__( self, crocoddyl.StateVector(6), nu=2, nr=12, ng=2 ) self.unone = np.zeros(self.nu) - self.span = .2 - self.mass = 2. + self.span = 0.2 + self.mass = 2.0 self.g = 10 - self.inertia = self.mass*self.span**2 + self.inertia = self.mass * self.span**2 self.costWeights = [ - 0.1, # x - 0.1, # z - .10, # s - .10, # c - 0.001, # vx - 0.001, # vz - 0.001, # w - 0.0, # fr - 0.0, # fl - 0.001,0.001,0.001, # a + 0.1, # x + 0.1, # z + 0.10, # s + 0.10, # c + 0.001, # vx + 0.001, # vz + 0.001, # w + 0.0, # fr + 0.0, # fl + 0.001, + 0.001, + 0.001, # a ] # sin, 1-cos, x, xdot, thdot, f def calc(self, data, x, u=None): if u is None: u = model.unone # Getting the state and control variables - x1,x2,th,v1,v2,w = x - fr,fl = u + x1, x2, th, v1, v2, w = x + fr, fl = u # Shortname for system parameters - mass,span,g,inertia=self.mass,self.span,self.g,self.inertia + mass, span, g, inertia = self.mass, self.span, self.g, self.inertia s, c = np.sin(th), np.cos(th) # Defining the equation of motions # Force (fx,fz,tauy) in local frame - loc_f = np.array([0, fr+fl, (fl-fr)*span]) + loc_f = np.array([0, fr + fl, (fl - fr) * span]) # Acceleration (x,z,th) in world frame - data.xout = np.array([ - -loc_f[1]*s/mass, - loc_f[1]*c/mass - g, - loc_f[2]/inertia - ]) + data.xout = np.array( + [-loc_f[1] * s / mass, loc_f[1] * c / mass - g, loc_f[2] / inertia] + ) # Computing the cost residual and value - data.r = self.costWeights * np.array([x1, x2, s, 1 - c, - v1, v2, w, - fr, fl, - data.xout[0], data.xout[1], data.xout[2] ]) - data.cost = 0.5 * sum(data.r ** 2) + data.r = self.costWeights * np.array( + [ + x1, + x2, + s, + 1 - c, + v1, + v2, + w, + fr, + fl, + data.xout[0], + data.xout[1], + data.xout[2], + ] + ) + data.cost = 0.5 * sum(data.r**2) data.g = np.array([u[0], u[1]]) @@ -90,6 +100,8 @@ def calcDiff(self, data, x, u=None): # Advance user might implement the derivatives. Here # we will rely on finite differences. pass + + # %end_jupyter_snippet # %jupyter_snippet dam_with_bounds @@ -114,13 +126,13 @@ def calcDiff(self, data, x, u=None): terminalDamND = crocoddyl.DifferentialActionModelNumDiff(terminalDam, True) terminalIam = crocoddyl.IntegratedActionModelEuler(terminalDamND) -terminalDam.costWeights[0] = 100 # horizontal position -terminalDam.costWeights[1] = 100 # vertical position -terminalDam.costWeights[2] = 100.0 # angle sin (first order) -terminalDam.costWeights[3] = 100.0 # angle cos (second order) -terminalDam.costWeights[4] = 100 # horizontal velocity -terminalDam.costWeights[5] = 100 # vertical velocity -terminalDam.costWeights[6] = 100 # angular velocity +terminalDam.costWeights[0] = 100 # horizontal position +terminalDam.costWeights[1] = 100 # vertical position +terminalDam.costWeights[2] = 100.0 # angle sin (first order) +terminalDam.costWeights[3] = 100.0 # angle cos (second order) +terminalDam.costWeights[4] = 100 # horizontal velocity +terminalDam.costWeights[5] = 100 # vertical velocity +terminalDam.costWeights[6] = 100 # angular velocity # %end_jupyter_snippet ### PROBLEM DEFINITION @@ -139,20 +151,20 @@ def calcDiff(self, data, x, u=None): csolver.eps_abs = 1e-5 csolver.eps_rel = 0.0 csolver.max_qp_iters = 10000 - + ### SOLVE THE PROBLEM -done = csolver.solve([x0]*(T+1), [np.zeros(2)]*T, MAX_ITER) -assert(done) +done = csolver.solve([x0] * (T + 1), [np.zeros(2)] * T, MAX_ITER) +assert done # %end_jupyter_snippet -### PLOT +### PLOT crocoddyl.plotOCSolution(csolver.xs, csolver.us, figIndex=1, show=False) plotBicopterSolution(list(csolver.xs)[::3]) -print('Type plt.show() to display the result.') +print("Type plt.show() to display the result.") # Animate the solution in meshcat viz = ViewerBicopter() -viz.displayTrajectory(csolver.xs,timeStep) +viz.displayTrajectory(csolver.xs, timeStep) diff --git a/next/tp5/bicopter_utils.py b/next/tp5/bicopter_utils.py index 42e033e..0cc5eed 100644 --- a/next/tp5/bicopter_utils.py +++ b/next/tp5/bicopter_utils.py @@ -1,86 +1,89 @@ import matplotlib.pyplot as plt import numpy as np -def plotBicopter(x,f=None,pltAx=None): - ''' + +def plotBicopter(x, f=None, pltAx=None): + """ Plot a bicopter of a 2D matplotlib, with arrows showing the thrust. - ''' + """ if pltAx is None: - pltAx=plt.gca() + pltAx = plt.gca() - g=10 - span = .2 - mass = 2. - force_scale=.5/mass/g + g = 10 + span = 0.2 + mass = 2.0 + force_scale = 0.5 / mass / g - f = [mass*g/2,mass*g/2] if f is None or len(f)==0 else f + f = [mass * g / 2, mass * g / 2] if f is None or len(f) == 0 else f a, b, th = x[:3] c, s = np.cos(th), np.sin(th) - fr,fl = f[0]*force_scale, f[1]*force_scale + fr, fl = f[0] * force_scale, f[1] * force_scale refs = [ - pltAx.arrow(a + span * c, b + span * s, - -s * fr, c * fr, head_width=.05), - pltAx.arrow(a - span * c, b - span * s, - -s * fl, c * fl, head_width=.05,color='r') + pltAx.arrow(a + span * c, b + span * s, -s * fr, c * fr, head_width=0.05), + pltAx.arrow( + a - span * c, b - span * s, -s * fl, c * fl, head_width=0.05, color="r" + ), ] return refs -def plotBicopterSolution(xs, pltAx=None,show=False): - ''' +def plotBicopterSolution(xs, pltAx=None, show=False): + """ Plot a sequence of bicopters by calling iterativelly plotBicopter. If need be, create the figure window. - ''' - if show=='interactive': + """ + if show == "interactive": plt.ion() if pltAx is None: - f,pltAx = plt.subplots(1,1, figsize=(6.4, 6.4)) + f, pltAx = plt.subplots(1, 1, figsize=(6.4, 6.4)) for x in xs: - plotBicopter(x,[],pltAx) - pltAx.axis([-2, 2., -2., 2.]) - pltAx.set_title('Bicopter trajectory') - pltAx.set_xlabel('horizontal x1 (m)') - pltAx.set_ylabel('verical x2 (m)') - if show==True: + plotBicopter(x, [], pltAx) + pltAx.axis([-2, 2.0, -2.0, 2.0]) + pltAx.set_title("Bicopter trajectory") + pltAx.set_xlabel("horizontal x1 (m)") + pltAx.set_ylabel("verical x2 (m)") + if show is True: plt.show() - + class ViewerBicopter: - ''' + """ Wrapper on meshcat to display a 3d object representing a (quad) bicopter. Call display(x) to display one frame, and displayTrajectories for a sequence. - ''' + """ + def __init__(self): - ''' + """ Init using example-robot-data to get the mesh of the quadcopter Hector, and meshcat to render it. - ''' + """ import example_robot_data as robex - from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer import pinocchio as pin + from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer - hector=robex.load('hector') - self.viz=MeshcatVisualizer(hector) - self.gname = self.viz.getViewerNodeName(self.viz.visual_model.geometryObjects[0], - pin.VISUAL) + hector = robex.load("hector") + self.viz = MeshcatVisualizer(hector) + self.gname = self.viz.getViewerNodeName( + self.viz.visual_model.geometryObjects[0], pin.VISUAL + ) - def display(self,x): - ''' + def display(self, x): + """ Display one pose of the quadcopter in meshcat. - ''' + """ import pinocchio as pin - M = pin.SE3(pin.utils.rotate('x',-x[2]), - np.array([0,x[0],x[1]])) - self.viz.applyConfiguration(self.gname,M) + M = pin.SE3(pin.utils.rotate("x", -x[2]), np.array([0, x[0], x[1]])) + self.viz.applyConfiguration(self.gname, M) - def displayTrajectory(self,xs,timeStep): - '''Display an animation showing a trajectory of a bicopter, + def displayTrajectory(self, xs, timeStep): + """Display an animation showing a trajectory of a bicopter, xs is a list-type object containing bicopter states [x,z,th] - and timeStep is used to control the lag of the animation. - ''' + and timeStep is used to control the lag of the animation. + """ import time + for x in xs: self.display(x) time.sleep(timeStep) diff --git a/next/tp5/generated/bicopter_toward_origin_dam_header b/next/tp5/generated/bicopter_toward_origin_dam_header index 883db62..76a3f93 100644 --- a/next/tp5/generated/bicopter_toward_origin_dam_header +++ b/next/tp5/generated/bicopter_toward_origin_dam_header @@ -2,7 +2,7 @@ class DifferentialActionModelBicopter(crocoddyl.DifferentialActionModelAbstract) def __init__(self): ''' - Init on top of the DAM class. + Init on top of the DAM class. Mostly set up the hyperparameters of this model (mass, length, cost, etc). ng=0 says we have 0 constraints. ''' diff --git a/next/tp5/generated/bicopter_toward_origin_dam_test b/next/tp5/generated/bicopter_toward_origin_dam_test index b6b7779..ec66431 100644 --- a/next/tp5/generated/bicopter_toward_origin_dam_test +++ b/next/tp5/generated/bicopter_toward_origin_dam_test @@ -1,5 +1,5 @@ # Create a local DAM data for testing the implementation dad = dam.createData() x = dam.state.rand() -u = np.array([12,8]) +u = np.array([12,8]) dam.calc(dad,x,u) diff --git a/next/tp5/generated/bicopter_toward_origin_with_constraints_dam b/next/tp5/generated/bicopter_toward_origin_with_constraints_dam index 39701d7..dc74da9 100644 --- a/next/tp5/generated/bicopter_toward_origin_with_constraints_dam +++ b/next/tp5/generated/bicopter_toward_origin_with_constraints_dam @@ -2,7 +2,7 @@ class DifferentialActionModelBicopter(crocoddyl.DifferentialActionModelAbstract) def __init__(self): ''' - Init on top of the DAM class. + Init on top of the DAM class. Mostly set up the hyperparameters of this model (mass, length, cost, etc). ''' crocoddyl.DifferentialActionModelAbstract.__init__( diff --git a/next/tp5/generated/bicopter_toward_origin_with_constraints_ocp b/next/tp5/generated/bicopter_toward_origin_with_constraints_ocp index b837d9c..df638b8 100644 --- a/next/tp5/generated/bicopter_toward_origin_with_constraints_ocp +++ b/next/tp5/generated/bicopter_toward_origin_with_constraints_ocp @@ -11,7 +11,7 @@ csolver.filter_size = MAX_ITER csolver.eps_abs = 1e-5 csolver.eps_rel = 0.0 csolver.max_qp_iters = 10000 - + ### SOLVE THE PROBLEM done = csolver.solve([x0]*(T+1), [np.zeros(2)]*T, MAX_ITER) assert(done) diff --git a/next/tp5/generated/panda_reaches_with_constraints_solver b/next/tp5/generated/panda_reaches_with_constraints_solver index c0943d7..ae368d9 100644 --- a/next/tp5/generated/panda_reaches_with_constraints_solver +++ b/next/tp5/generated/panda_reaches_with_constraints_solver @@ -1,7 +1,7 @@ solver = mim_solvers.SolverCSQP(problem) -solver.with_callbacks = True +solver.with_callbacks = True solver.termination_tolerance = 1e-3 # Termination criteria (KKT residual) solver.max_qp_iters = 1000 # Maximum number of QP iteration -solver.eps_abs = 1e-5 # QP termination absolute criteria, 1e-9 +solver.eps_abs = 1e-5 # QP termination absolute criteria, 1e-9 solver.eps_rel = 0. # QP termination absolute criteria solver.use_filter_line_search = True # True by default, False = use merit function diff --git a/next/tp5/panda_reaches_a_single_target.py b/next/tp5/panda_reaches_a_single_target.py index 64ca8e9..e9ddbb5 100644 --- a/next/tp5/panda_reaches_a_single_target.py +++ b/next/tp5/panda_reaches_a_single_target.py @@ -1,4 +1,4 @@ -''' +""" In this example test, we will solve the reaching-goal task with the Panda arm. For that, we use the forward dynamics (with its analytical derivatives) based on Pinocchio and featured in the Pinocchio-based front-end implementation of Crocoddyl. The main code is @@ -7,22 +7,23 @@ For temporal integration, we use an Euler implicit integration scheme. This example is based on https://github.com/Gepetto/supaero2023/blob/main/tp5/arm_example.py -''' +""" # %jupyter_snippet import import crocoddyl -import pinocchio as pin -import numpy as np import example_robot_data as robex -import matplotlib.pylab as plt +import numpy as np +import pinocchio as pin + # %end_jupyter_snippet # %jupyter_snippet robexload # First, let's load the Pinocchio model for the Panda arm. -robot = robex.load('panda') +robot = robex.load("panda") # The 2 last joints are for the fingers, not important in arm motion, freeze them -robot.model,[robot.visual_model,robot.collision_model] = \ - pin.buildReducedModel(robot.model,[robot.visual_model,robot.collision_model],[8,9],robot.q0) +robot.model, [robot.visual_model, robot.collision_model] = pin.buildReducedModel( + robot.model, [robot.visual_model, robot.collision_model], [8, 9], robot.q0 +) robot.q0 = robot.q0[:7].copy() # %end_jupyter_snippet @@ -31,24 +32,27 @@ HORIZON_LENGTH = 100 TIME_STEP = 1e-2 FRAME_TIP = robot.model.getFrameId("panda_hand_tcp") -GOAL_POSITION = np.array([.2,0.6,.5]) -GOAL_PLACEMENT = pin.SE3(pin.utils.rpyToMatrix(-np.pi,-1.5,1.5), GOAL_POSITION) -REACH_DIMENSION = "3d" # "6d" +GOAL_POSITION = np.array([0.2, 0.6, 0.5]) +GOAL_PLACEMENT = pin.SE3(pin.utils.rpyToMatrix(-np.pi, -1.5, 1.5), GOAL_POSITION) +REACH_DIMENSION = "3d" # "6d" # %end_jupyter_snippet # Configure viewer to vizualize the robot and a green box to feature the goal placement. # %jupyter_snippet viz from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer + viz = MeshcatVisualizer(robot) viz.display(robot.q0) -viz.addBox('world/goal',[.1,.1,.1],[0,1,0,1]) -viz.applyConfiguration('world/goal',GOAL_PLACEMENT) +viz.addBox("world/goal", [0.1, 0.1, 0.1], [0, 1, 0, 1]) +viz.applyConfiguration("world/goal", GOAL_PLACEMENT) # %end_jupyter_snippet # %jupyter_snippet robot_model # Set robot model robot_model = robot.model -robot_model.armature = np.ones(robot.model.nv)*2 # Arbitrary value representing the true armature +robot_model.armature = ( + np.ones(robot.model.nv) * 2 +) # Arbitrary value representing the true armature robot_model.q0 = robot.q0.copy() robot_model.x0 = np.concatenate([robot_model.q0, np.zeros(robot_model.nv)]) # %end_jupyter_snippet @@ -69,41 +73,53 @@ # %jupyter_snippet cost_goal if REACH_DIMENSION == "3d": # Cost for 3d tracking || p(q) - pref ||**2 - goalTrackingRes = crocoddyl.ResidualModelFrameTranslation(state,FRAME_TIP,GOAL_POSITION) - goalTrackingWeights = crocoddyl.ActivationModelWeightedQuad(np.array([1,1,1])) + goalTrackingRes = crocoddyl.ResidualModelFrameTranslation( + state, FRAME_TIP, GOAL_POSITION + ) + goalTrackingWeights = crocoddyl.ActivationModelWeightedQuad(np.array([1, 1, 1])) elif REACH_DIMENSION == "6d": # Cost for 6d tracking || log( M(q)^-1 Mref ) ||**2 - goalTrackingRes = crocoddyl.ResidualModelFramePlacement(state,FRAME_TIP,GOAL_PLACEMENT) - goalTrackingWeights = crocoddyl.ActivationModelWeightedQuad(np.array([1,1,1, 1,1,1])) + goalTrackingRes = crocoddyl.ResidualModelFramePlacement( + state, FRAME_TIP, GOAL_PLACEMENT + ) + goalTrackingWeights = crocoddyl.ActivationModelWeightedQuad( + np.array([1, 1, 1, 1, 1, 1]) + ) else: - assert( REACH_DIMENSION=="3d" or REACH_DIMENSION=="6d" ) -goalTrackingCost = crocoddyl.CostModelResidual(state,goalTrackingWeights,goalTrackingRes) -runningCostModel.addCost("gripperPose", goalTrackingCost, .001) + assert REACH_DIMENSION == "3d" or REACH_DIMENSION == "6d" +goalTrackingCost = crocoddyl.CostModelResidual( + state, goalTrackingWeights, goalTrackingRes +) +runningCostModel.addCost("gripperPose", goalTrackingCost, 0.001) terminalCostModel.addCost("gripperPose", goalTrackingCost, 4) # %end_jupyter_snippet - + # %jupyter_snippet cost_xreg # Cost for state regularization || x - x* ||**2 # We set up different values for the integral cost and terminal cost term. # Regularization is stronger on position than velocity (to account for typical unit scale) -xRegWeights = crocoddyl.ActivationModelWeightedQuad(np.array([1,1,1,1,1,1,1, .1,.1,.1,.1,.1,.1,.1])) -xRegRes = crocoddyl.ResidualModelState(state,robot_model.x0) -xRegCost = crocoddyl.CostModelResidual(state,xRegWeights,xRegRes) +xRegWeights = crocoddyl.ActivationModelWeightedQuad( + np.array([1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) +) +xRegRes = crocoddyl.ResidualModelState(state, robot_model.x0) +xRegCost = crocoddyl.CostModelResidual(state, xRegWeights, xRegRes) runningCostModel.addCost("xReg", xRegCost, 1e-2) # Terminal cost for state regularization || x - x* ||**2 # Require more strictly a small velocity at task end (but we don't car for the position) -xRegWeightsT=crocoddyl.ActivationModelWeightedQuad(np.array([.5,.5,.5,.5,.5,.5,.5, 5.,5.,5.,5.,5.,5.,5.])) -xRegResT = crocoddyl.ResidualModelState(state,robot_model.x0) -xRegCostT = crocoddyl.CostModelResidual(state,xRegWeightsT,xRegResT) -terminalCostModel.addCost("xReg", xRegCostT, .1) +xRegWeightsT = crocoddyl.ActivationModelWeightedQuad( + np.array([0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0]) +) +xRegResT = crocoddyl.ResidualModelState(state, robot_model.x0) +xRegCostT = crocoddyl.CostModelResidual(state, xRegWeightsT, xRegResT) +terminalCostModel.addCost("xReg", xRegCostT, 0.1) # %end_jupyter_snippet # %jupyter_snippet cost_ureg # Cost for control regularization || u - g(q) ||**2 uRegRes = crocoddyl.ResidualModelControlGrav(state) -uRegCost = crocoddyl.CostModelResidual(state,uRegRes) +uRegCost = crocoddyl.CostModelResidual(state, uRegRes) runningCostModel.addCost("uReg", uRegCost, 1e-5) # %end_jupyter_snippet @@ -116,11 +132,19 @@ actuationModel = crocoddyl.ActuationModelFull(state) # Running model composing the costs, the differential equations of motion and the integrator. runningModel = crocoddyl.IntegratedActionModelEuler( - crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, runningCostModel), TIME_STEP) + crocoddyl.DifferentialActionModelFreeFwdDynamics( + state, actuationModel, runningCostModel + ), + TIME_STEP, +) runningModel.differential.armature = robot_model.armature # Terminal model following the same logic, although the integration is here trivial. terminalModel = crocoddyl.IntegratedActionModelEuler( - crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, terminalCostModel), 0.) + crocoddyl.DifferentialActionModelFreeFwdDynamics( + state, actuationModel, terminalCostModel + ), + 0.0, +) terminalModel.differential.armature = robot_model.armature # %end_jupyter_snippet @@ -128,22 +152,26 @@ # For this optimal control problem, we define HORIZON_LENGTH knots (or running action # models) plus a terminal knot # %jupyter_snippet shoot -problem = crocoddyl.ShootingProblem(robot_model.x0, [runningModel] * HORIZON_LENGTH, terminalModel) +problem = crocoddyl.ShootingProblem( + robot_model.x0, [runningModel] * HORIZON_LENGTH, terminalModel +) # %end_jupyter_snippet # %jupyter_snippet solve # Solving it using DDP # Create the DDP solver for this OC problem, verbose traces, with a logger ddp = crocoddyl.SolverDDP(problem) -ddp.setCallbacks([ - crocoddyl.CallbackLogger(), - crocoddyl.CallbackVerbose(), -]) +ddp.setCallbacks( + [ + crocoddyl.CallbackLogger(), + crocoddyl.CallbackVerbose(), + ] +) # Solving it with the DDP algorithm -ddp.solve([],[],1000) # xs_init,us_init,maxiter +ddp.solve([], [], 1000) # xs_init,us_init,maxiter # %end_jupyter_snippet -#assert( ddp.stop == 1.9384159634520916e-10 ) +# assert( ddp.stop == 1.9384159634520916e-10 ) # %jupyter_snippet plot # Plotting the solution and the DDP convergence @@ -160,22 +188,25 @@ show=False, ) # %end_jupyter_snippet -print('Type plt.show() to display the plots') +print("Type plt.show() to display the plots") # %jupyter_snippet animate # # Visualizing the solution in gepetto-viewer -viz.play([x[:robot.model.nq] for x in ddp.xs],TIME_STEP) +viz.play([x[: robot.model.nq] for x in ddp.xs], TIME_STEP) # %end_jupyter_snippet ### TEST ZONE ############################################################ ### This last part is to automatically validate the versions of this example. import unittest + + class LocalTest(unittest.TestCase): def test_logs(self): print(self.__class__.__name__) - self.assertTrue( len(ddp.xs) == len(ddp.us)+1 ) - self.assertTrue( np.allclose(ddp.xs[0],ddp.problem.x0) ) - self.assertTrue( ddp.stop<1e-6 ) - + self.assertTrue(len(ddp.xs) == len(ddp.us) + 1) + self.assertTrue(np.allclose(ddp.xs[0], ddp.problem.x0)) + self.assertTrue(ddp.stop < 1e-6) + + if __name__ == "__main__": LocalTest().test_logs() diff --git a/next/tp5/panda_reaches_with_constraints.py b/next/tp5/panda_reaches_with_constraints.py index 0c49a61..c8fd7aa 100644 --- a/next/tp5/panda_reaches_with_constraints.py +++ b/next/tp5/panda_reaches_with_constraints.py @@ -1,4 +1,4 @@ -''' +""" In this example test, we will solve the reaching-goal task with the Panda arm. For that, we use the forward dynamics (with its analytical derivatives) based on Pinocchio and featured in the Pinocchio-based front-end implementation of Crocoddyl. The main code is @@ -7,46 +7,50 @@ For temporal integration, we use an Euler implicit integration scheme. This example is based on https://github.com/Gepetto/supaero2023/blob/main/tp5/arm_example.py -''' +""" import crocoddyl -import pinocchio as pin -import numpy as np import example_robot_data as robex import matplotlib.pylab as plt import mim_solvers +import numpy as np +import pinocchio as pin # First, let's load the Pinocchio model for the Panda arm. -robot = robex.load('panda') +robot = robex.load("panda") # The 2 last joints are for the fingers, not important in arm motion, freeze them -robot.model,[robot.visual_model,robot.collision_model] = \ - pin.buildReducedModel(robot.model,[robot.visual_model,robot.collision_model],[8,9],robot.q0) +robot.model, [robot.visual_model, robot.collision_model] = pin.buildReducedModel( + robot.model, [robot.visual_model, robot.collision_model], [8, 9], robot.q0 +) robot.q0 = robot.q0[:7].copy() # Hyperparameters of the movement HORIZON_LENGTH = 100 TIME_STEP = 1e-2 FRAME_TIP = robot.model.getFrameId("panda_hand_tcp") -GOAL_POSITION = np.array([.2,0.5,.5]) -GOAL_PLACEMENT = pin.SE3(pin.utils.rpyToMatrix(-np.pi,0,np.pi/4), GOAL_POSITION) -GOAL_POSITION = np.array([.2,0.6,.5]) -GOAL_PLACEMENT = pin.SE3(pin.utils.rpyToMatrix(-np.pi,-1.5,1.5), GOAL_POSITION) -REACH_DIMENSION = "6d" # "3d" +GOAL_POSITION = np.array([0.2, 0.5, 0.5]) +GOAL_PLACEMENT = pin.SE3(pin.utils.rpyToMatrix(-np.pi, 0, np.pi / 4), GOAL_POSITION) +GOAL_POSITION = np.array([0.2, 0.6, 0.5]) +GOAL_PLACEMENT = pin.SE3(pin.utils.rpyToMatrix(-np.pi, -1.5, 1.5), GOAL_POSITION) +REACH_DIMENSION = "6d" # "3d" # %jupyter_snippet hyperparameters -X_WALL_LOWER = .25 -X_WALL_UPPER = .35 +X_WALL_LOWER = 0.25 +X_WALL_UPPER = 0.35 # %end_jupyter_snippet # Configure viewer to vizualize the robot and a green box to feature the goal placement. from supaero2024.meshcat_viewer_wrapper import MeshcatVisualizer + viz = MeshcatVisualizer(robot) viz.display(robot.q0) -viz.addBox('world/goal',[.1,.1,.1],[0,1,0,1]) -viz.applyConfiguration('world/goal',GOAL_PLACEMENT) +viz.addBox("world/goal", [0.1, 0.1, 0.1], [0, 1, 0, 1]) +viz.applyConfiguration("world/goal", GOAL_PLACEMENT) # Set robot model robot_model = robot.model -robot_model.armature = np.ones(robot.model.nv)*2 # Arbitrary value representing the true armature +robot_model.armature = ( + np.ones(robot.model.nv) * 2 +) # Arbitrary value representing the true armature robot_model.q0 = robot.q0.copy() robot_model.x0 = np.concatenate([robot_model.q0, np.zeros(robot_model.nv)]) @@ -61,37 +65,49 @@ # You can ask the end effector to reach a position (3d) or a placement (6d) if REACH_DIMENSION == "3d": # Cost for 3d tracking || p(q) - pref ||**2 - goalTrackingRes = crocoddyl.ResidualModelFrameTranslation(state,FRAME_TIP,GOAL_POSITION) - goalTrackingWeights = crocoddyl.ActivationModelWeightedQuad(np.array([1,1,1])) + goalTrackingRes = crocoddyl.ResidualModelFrameTranslation( + state, FRAME_TIP, GOAL_POSITION + ) + goalTrackingWeights = crocoddyl.ActivationModelWeightedQuad(np.array([1, 1, 1])) elif REACH_DIMENSION == "6d": # Cost for 6d tracking || log( M(q)^-1 Mref ) ||**2 - goalTrackingRes = crocoddyl.ResidualModelFramePlacement(state,FRAME_TIP,GOAL_PLACEMENT) - goalTrackingWeights = crocoddyl.ActivationModelWeightedQuad(np.array([1,1,1, 1,1,1])) + goalTrackingRes = crocoddyl.ResidualModelFramePlacement( + state, FRAME_TIP, GOAL_PLACEMENT + ) + goalTrackingWeights = crocoddyl.ActivationModelWeightedQuad( + np.array([1, 1, 1, 1, 1, 1]) + ) else: - assert( REACH_DIMENSION=="3d" or REACH_DIMENSION=="6d" ) -goalTrackingCost = crocoddyl.CostModelResidual(state,goalTrackingWeights,goalTrackingRes) -runningCostModel.addCost("gripperPose", goalTrackingCost, .001) + assert REACH_DIMENSION == "3d" or REACH_DIMENSION == "6d" +goalTrackingCost = crocoddyl.CostModelResidual( + state, goalTrackingWeights, goalTrackingRes +) +runningCostModel.addCost("gripperPose", goalTrackingCost, 0.001) terminalCostModel.addCost("gripperPose", goalTrackingCost, 4) - + # Cost for state regularization || x - x* ||**2 # We set up different values for the integral cost and terminal cost term. # Regularization is stronger on position than velocity (to account for typical unit scale) -xRegWeights = crocoddyl.ActivationModelWeightedQuad(np.array([1,1,1,1,1,1,1, .1,.1,.1,.1,.1,.1,.1])) -xRegRes = crocoddyl.ResidualModelState(state,robot_model.x0) -xRegCost = crocoddyl.CostModelResidual(state,xRegWeights,xRegRes) +xRegWeights = crocoddyl.ActivationModelWeightedQuad( + np.array([1, 1, 1, 1, 1, 1, 1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) +) +xRegRes = crocoddyl.ResidualModelState(state, robot_model.x0) +xRegCost = crocoddyl.CostModelResidual(state, xRegWeights, xRegRes) runningCostModel.addCost("xReg", xRegCost, 1e-2) # Terminal cost for state regularization || x - x* ||**2 # Require more strictly a small velocity at task end (but we don't car for the position) -xRegWeightsT=crocoddyl.ActivationModelWeightedQuad(np.array([.5,.5,.5,.5,.5,.5,.5, 5.,5.,5.,5.,5.,5.,5.])) -xRegResT = crocoddyl.ResidualModelState(state,robot_model.x0) -xRegCostT = crocoddyl.CostModelResidual(state,xRegWeightsT,xRegResT) -terminalCostModel.addCost("xReg", xRegCostT, .1) +xRegWeightsT = crocoddyl.ActivationModelWeightedQuad( + np.array([0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0]) +) +xRegResT = crocoddyl.ResidualModelState(state, robot_model.x0) +xRegCostT = crocoddyl.CostModelResidual(state, xRegWeightsT, xRegResT) +terminalCostModel.addCost("xReg", xRegCostT, 0.1) # Cost for control regularization || u - g(q) ||**2 uRegRes = crocoddyl.ResidualModelControlGrav(state) -uRegCost = crocoddyl.CostModelResidual(state,uRegRes) +uRegCost = crocoddyl.CostModelResidual(state, uRegRes) runningCostModel.addCost("uReg", uRegCost, 1e-5) @@ -115,7 +131,6 @@ # %end_jupyter_snippet - # %jupyter_snippet iam # Next, we need to create the running and terminal action model. # The forward dynamics (computed using ABA) are implemented @@ -126,18 +141,26 @@ # Running model composing the costs, the differential equations of motion and the integrator. runningModel = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( - state, actuationModel, runningCostModel, runningConstraints), - TIME_STEP) + state, actuationModel, runningCostModel, runningConstraints + ), + TIME_STEP, +) runningModel.differential.armature = robot_model.armature # Specific unconstrained initial model runningModel_init = crocoddyl.IntegratedActionModelEuler( crocoddyl.DifferentialActionModelFreeFwdDynamics( - state, actuationModel, runningCostModel), - TIME_STEP) + state, actuationModel, runningCostModel + ), + TIME_STEP, +) runningModel.differential.armature = robot_model.armature # Terminal model following the same logic, although the integration is here trivial. terminalModel = crocoddyl.IntegratedActionModelEuler( - crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, terminalCostModel), 0.) + crocoddyl.DifferentialActionModelFreeFwdDynamics( + state, actuationModel, terminalCostModel + ), + 0.0, +) terminalModel.differential.armature = robot_model.armature # %end_jupyter_snippet @@ -145,49 +168,56 @@ # For this optimal control problem, we define HORIZON_LENGTH knots (or running action # models) plus a terminal knot # %jupyter_snippet shoot -problem = crocoddyl.ShootingProblem(robot_model.x0, - [runningModel_init] + [runningModel] * (HORIZON_LENGTH - 1), - terminalModel) +problem = crocoddyl.ShootingProblem( + robot_model.x0, + [runningModel_init] + [runningModel] * (HORIZON_LENGTH - 1), + terminalModel, +) # %end_jupyter_snippet # %jupyter_snippet solver solver = mim_solvers.SolverCSQP(problem) -solver.with_callbacks = True -solver.termination_tolerance = 1e-3 # Termination criteria (KKT residual) -solver.max_qp_iters = 1000 # Maximum number of QP iteration -solver.eps_abs = 1e-5 # QP termination absolute criteria, 1e-9 -solver.eps_rel = 0. # QP termination absolute criteria -solver.use_filter_line_search = True # True by default, False = use merit function +solver.with_callbacks = True +solver.termination_tolerance = 1e-3 # Termination criteria (KKT residual) +solver.max_qp_iters = 1000 # Maximum number of QP iteration +solver.eps_abs = 1e-5 # QP termination absolute criteria, 1e-9 +solver.eps_rel = 0.0 # QP termination absolute criteria +solver.use_filter_line_search = True # True by default, False = use merit function # %end_jupyter_snippet # %jupyter_snippet solve_and_plot # Solving it with the DDP algorithm -solver.solve([],[],1000) # xs_init,us_init,maxiter +solver.solve([], [], 1000) # xs_init,us_init,maxiter -ees = [ d.differential.pinocchio.oMf[FRAME_TIP].translation for d in solver.problem.runningDatas ] +ees = [ + d.differential.pinocchio.oMf[FRAME_TIP].translation + for d in solver.problem.runningDatas +] plt.plot(ees) -plt.plot([0,HORIZON_LENGTH],[X_WALL_UPPER,X_WALL_UPPER],'b--') -plt.plot([0,HORIZON_LENGTH],[X_WALL_LOWER,X_WALL_LOWER],'b--') -plt.legend(['x', 'y', 'z']) +plt.plot([0, HORIZON_LENGTH], [X_WALL_UPPER, X_WALL_UPPER], "b--") +plt.plot([0, HORIZON_LENGTH], [X_WALL_LOWER, X_WALL_LOWER], "b--") +plt.legend(["x", "y", "z"]) # %end_jupyter_snippet -print('Type plt.show() to display the result.') +print("Type plt.show() to display the result.") # %jupyter_snippet animate # Visualizing the solution in gepetto-viewer -viz.play([x[:robot.model.nq] for x in solver.xs],TIME_STEP) +viz.play([x[: robot.model.nq] for x in solver.xs], TIME_STEP) # %end_jupyter_snippet # ## TEST ZONE ############################################################ # ## This last part is to automatically validate the versions of this example. import unittest + + class LocalTest(unittest.TestCase): def test_logs(self): print(self.__class__.__name__) - self.assertTrue( len(solver.xs) == len(solver.us)+1 ) - self.assertTrue( np.allclose(solver.xs[0],solver.problem.x0) ) - self.assertTrue( solver.stoppingCriteria()<1e-6 ) - + self.assertTrue(len(solver.xs) == len(solver.us) + 1) + self.assertTrue(np.allclose(solver.xs[0], solver.problem.x0)) + self.assertTrue(solver.stoppingCriteria() < 1e-6) + + if __name__ == "__main__": LocalTest().test_logs() - diff --git a/next/tp5/unicycle_toward_origin.py b/next/tp5/unicycle_toward_origin.py index d9f325b..45d9f3d 100644 --- a/next/tp5/unicycle_toward_origin.py +++ b/next/tp5/unicycle_toward_origin.py @@ -1,13 +1,13 @@ +import unittest + import crocoddyl import numpy as np -import matplotlib.pylab as plt -import unittest from tp5.unicycle_utils import plotUnicycleSolution # %jupyter_snippet hyperparams ### HYPER PARAMS: horizon and initial state -T = 100 -x0 = np.array([-1,-1,1]) +T = 100 +x0 = np.array([-1, -1, 1]) # %end_jupyter_snippet ### PROBLEM DEFINITION @@ -16,14 +16,11 @@ # %jupyter_snippet termmodel model_term = crocoddyl.ActionModelUnicycle() -model_term.costWeights = np.array([ - 100, # state weight - 0 # control weight -]).T +model_term.costWeights = np.array([100, 0]).T # state weight # control weight # %end_jupyter_snippet # Define the optimal control problem. -problem = crocoddyl.ShootingProblem(x0, [ model ] * T, model_term) +problem = crocoddyl.ShootingProblem(x0, [model] * T, model_term) # %jupyter_snippet ddp # Select the solver for this problem ddp = crocoddyl.SolverDDP(problem) @@ -38,10 +35,10 @@ # %jupyter_snippet solve done = ddp.solve() -assert(done) +assert done # %end_jupyter_snippet -### PLOT +### PLOT # %jupyter_snippet plotlog log = ddp.getCallbacks()[0] @@ -60,16 +57,18 @@ plotUnicycleSolution(log.xs) -print('Type plt.show() to display the result.') +print("Type plt.show() to display the result.") + ### TEST ZONE ############################################################ ### This last part is to automatically validate the versions of this example. class UnicycleTest(unittest.TestCase): def test_logs(self): print(self.__class__.__name__) - self.assertTrue( len(ddp.xs) == len(ddp.us)+1 ) - self.assertTrue( np.allclose(ddp.xs[0],ddp.problem.x0) ) - self.assertTrue( ddp.stop<1e-6 ) - + self.assertTrue(len(ddp.xs) == len(ddp.us) + 1) + self.assertTrue(np.allclose(ddp.xs[0], ddp.problem.x0)) + self.assertTrue(ddp.stop < 1e-6) + + if __name__ == "__main__": UnicycleTest().test_logs() diff --git a/next/tp5/unicycle_utils.py b/next/tp5/unicycle_utils.py index 312caa5..431cb2c 100644 --- a/next/tp5/unicycle_utils.py +++ b/next/tp5/unicycle_utils.py @@ -1,10 +1,11 @@ import matplotlib.pyplot as plt import numpy as np -def plotUnicycle(x,pltAx): - ''' + +def plotUnicycle(x, pltAx): + """ Plot a unicyle of a 2D matplotlib (top view), with fix-size arrows featuring the wheels. - ''' + """ sc, delta = 0.1, 0.1 a, b, th = x c, s = np.cos(th), np.sin(th) @@ -28,10 +29,10 @@ def plotUnicycle(x,pltAx): def plotUnicycleSolution(xs, pltAx=None): - ''' + """ Plot a sequence of unicyles by calling iterativelly plotUnicycle. If need be, create the figure window. - ''' + """ if pltAx is None: f, pltAx = plt.subplots(1, 1, figsize=(6.4, 6.4)) for x in xs: diff --git a/supaero2025/hppfcl_utils.py b/supaero2025/hppfcl_utils.py index 0c86020..2d27279 100644 --- a/supaero2025/hppfcl_utils.py +++ b/supaero2025/hppfcl_utils.py @@ -1,15 +1,16 @@ -''' +""" Some useful and simple helpers for hppfcl: - load_hppfcl_convex_from_stl_file -''' +""" import hppfcl + def load_hppfcl_convex_from_stl_file(path: str) -> hppfcl.ConvexBase: - ''' + """ Load a convex hppfcl object from .stl file whose path is given in argument. - ''' + """ shape: hppfcl.ConvexBase loader = hppfcl.MeshLoader() mesh_: hppfcl.BVHModelBase = loader.load(path) diff --git a/supaero2025/meshcat_viewer_wrapper/visualizer.py b/supaero2025/meshcat_viewer_wrapper/visualizer.py index dd7c732..2923c57 100644 --- a/supaero2025/meshcat_viewer_wrapper/visualizer.py +++ b/supaero2025/meshcat_viewer_wrapper/visualizer.py @@ -30,7 +30,7 @@ def materialFromColor(color): class MeshcatVisualizer(PMV): def __init__( - self, robot=None, model=None, collision_model=None, visual_model=None, url=None + self, robot=None, model=None, collision_model=None, visual_model=None, url=None ): if robot is not None: super().__init__(robot.model, robot.collision_model, robot.visual_model) @@ -41,7 +41,7 @@ def __init__( if url == "classical": url = "tcp://127.0.0.1:6000" print(f'*** You asked to start meshcat "classically" in {url}') - print('*** Did you start meshcat manually (meshcat-server)') + print("*** Did you start meshcat manually (meshcat-server)") print("Wrapper tries to connect to server <%s>" % url) server = meshcat.Visualizer(zmq_url=url) else: @@ -61,9 +61,9 @@ def addLine(self, name, point1, point2, color, linewidth=None): if linewidth is not None: # 1 by default set in the constructor of material # TODO: this does not seem to have any effect - material.linewidth=linewidth - points = np.block([[point1],[point2]]).T.astype(np.float32) - line = meshcat.geometry.Line(meshcat.geometry.PointsGeometry(points),material) + material.linewidth = linewidth + points = np.block([[point1], [point2]]).T.astype(np.float32) + line = meshcat.geometry.Line(meshcat.geometry.PointsGeometry(points), material) self.viewer[name].set_object(line) def addCylinder(self, name, length, radius, color=None): @@ -87,7 +87,7 @@ def applyConfiguration(self, name, placement): R = pin.Quaternion(np.reshape(placement[3:], [4, 1])).matrix() p = placement[:3] T = np.r_[np.c_[R, p], [[0, 0, 0, 1]]] - elif placement.shape == (4,4): + elif placement.shape == (4, 4): T = placement else: print("Error, np.shape of placement is not accepted") @@ -110,21 +110,29 @@ def __getitem__(self, name): # The next part of code is to reproduce the behavior of meshcat in pinocchio # 3 (version 2.99) inside pinocchio 2 (version 2.7). def _loadPrimitive(geometry_object: pin.GeometryObject): - ''' - Update the pin loadPrimitive function of Pinocchio2 by a code from + """ + Update the pin loadPrimitive function of Pinocchio2 by a code from pinocchio 2.99. - ''' + """ import warnings + from typing import Any, Dict, Union + import hppfcl import meshcat.geometry as mg - from typing import Optional, Any, Dict, Union, Type, Set - MsgType = Dict[str, Union[str, bytes, bool, float, 'MsgType']] - pin.ZAxis=np.array([0,0,1.]) - + + MsgType = Dict[str, Union[str, bytes, bool, float, "MsgType"]] + pin.ZAxis = np.array([0, 0, 1.0]) + class mgPlane(mg.Geometry): - """A plane of the given width and height. - """ - def __init__(self, width: float, height: float, widthSegments: float = 1, heightSegments: float = 1): + """A plane of the given width and height.""" + + def __init__( + self, + width: float, + height: float, + widthSegments: float = 1, + heightSegments: float = 1, + ): super().__init__() self.width = width self.height = height @@ -133,56 +141,77 @@ def __init__(self, width: float, height: float, widthSegments: float = 1, height def lower(self, object_data: Any) -> MsgType: return { - u"uuid": self.uuid, - u"type": u"PlaneGeometry", - u"width": self.width, - u"height": self.height, - u"widthSegments": self.widthSegments, - u"heightSegments": self.heightSegments, + "uuid": self.uuid, + "type": "PlaneGeometry", + "width": self.width, + "height": self.height, + "widthSegments": self.widthSegments, + "heightSegments": self.heightSegments, } # Cylinders need to be rotated - basic_three_js_transform = np.array([[1., 0., 0., 0.], - [0., 0., -1., 0.], - [0., 1., 0., 0.], - [0., 0., 0., 1.]]) - RotatedCylinder = type("RotatedCylinder", (mg.Cylinder,), {"intrinsic_transform": lambda self: basic_three_js_transform }) + basic_three_js_transform = np.array( + [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, -1.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) + RotatedCylinder = type( + "RotatedCylinder", + (mg.Cylinder,), + {"intrinsic_transform": lambda self: basic_three_js_transform}, + ) # Cones need to be rotated geom: hppfcl.ShapeBase = geometry_object.geometry if isinstance(geom, hppfcl.Capsule): - if hasattr(mg, 'TriangularMeshGeometry'): - obj = pin.visualize.meshcat_visualizer.createCapsule(2. * geom.halfLength, geom.radius) + if hasattr(mg, "TriangularMeshGeometry"): + obj = pin.visualize.meshcat_visualizer.createCapsule( + 2.0 * geom.halfLength, geom.radius + ) else: - obj = RotatedCylinder(2. * geom.halfLength, geom.radius) + obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius) elif isinstance(geom, hppfcl.Cylinder): - obj = RotatedCylinder(2. * geom.halfLength, geom.radius) + obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius) elif isinstance(geom, hppfcl.Cone): - obj = RotatedCylinder(2. * geom.halfLength, 0, geom.radius, 0) + obj = RotatedCylinder(2.0 * geom.halfLength, 0, geom.radius, 0) elif isinstance(geom, hppfcl.Box): - obj = mg.Box(pin.utils.npToTuple(2. * geom.halfSide)) + obj = mg.Box(pin.utils.npToTuple(2.0 * geom.halfSide)) elif isinstance(geom, hppfcl.Sphere): obj = mg.Sphere(geom.radius) elif isinstance(geom, hppfcl.Plane): To = np.eye(4) To[:3, 3] = geom.d * geom.n - TranslatedPlane = type("TranslatedPlane", (mg.Plane,), {"intrinsic_transform": lambda self: To}) + TranslatedPlane = type( + "TranslatedPlane", (mg.Plane,), {"intrinsic_transform": lambda self: To} + ) sx = geometry_object.meshScale[0] * 10 sy = geometry_object.meshScale[1] * 10 obj = TranslatedPlane(sx, sy) elif isinstance(geom, hppfcl.Ellipsoid): obj = mg.Ellipsoid(geom.radii) - elif isinstance(geom, (hppfcl.Plane,hppfcl.Halfspace)): - plane_transform : pin.SE3 = pin.SE3.Identity() + elif isinstance(geom, (hppfcl.Plane, hppfcl.Halfspace)): + plane_transform: pin.SE3 = pin.SE3.Identity() # plane_transform.translation[:] = geom.d # Does not work - plane_transform.rotation = pin.Quaternion.FromTwoVectors(pin.ZAxis,geom.n).toRotationMatrix() - TransformedPlane = type("TransformedPlane", (mgPlane,), {"intrinsic_transform": lambda self: plane_transform.homogeneous }) - obj = TransformedPlane(1000,1000) + plane_transform.rotation = pin.Quaternion.FromTwoVectors( + pin.ZAxis, geom.n + ).toRotationMatrix() + TransformedPlane = type( + "TransformedPlane", + (mgPlane,), + {"intrinsic_transform": lambda self: plane_transform.homogeneous}, + ) + obj = TransformedPlane(1000, 1000) elif isinstance(geom, hppfcl.ConvexBase): obj = pin.visualize.meshcat_visualizer.loadMesh(geom) else: - msg = "Unsupported geometry type for %s (%s)" % (geometry_object.name, type(geom) ) + msg = "Unsupported geometry type for %s (%s)" % ( + geometry_object.name, + type(geom), + ) warnings.warn(msg, category=UserWarning, stacklevel=2) obj = None