<jupyter_start><jupyter_text>IntroductionThis package provide C++ structures with python bindings used to define and store contact phases and contact sequences. The classes defined by this package are able to store a wide variety of data related to the locomotion of legged robots. This classes can be efficiently serialized and used to share motions or any "part" of motion (eg. contact plan or centroidal reference) between frameworks and users. In this package, the different classes do not have access to any robot model and only store the data given by an user. There is nothing to check that the different data stored are consistent together or valid (eg. there could be a contact placement for a feet defined at a given time, but also a wholebody configuration defined for the same time where the feet is at a different position).However, a couple of methods are available to check that part of the data are correctly defined, are continuous and/or have consistent time definition. See the last section of this notebook for more details.  Main classesThis package define 4 main classes representing physical concepts of legged locomotion. Contact Model, Contact Patch, Contact Phase and Contact Sequence.A Contact Model define the physical properties of a contact. A Contact Patch define completely a contact between a part of the robot and the environment, it contain a Contact Model. A Contact Phase is defined by a constant set of contacts, it contains one or more Contact Patches. Finally, a Contact Sequence is a sequence of Contact Phases. Contact ModelA Contact Model define the physical properties of a contact: the type of contact (Planar or Point), the physical positions of the contact points with respect to the "center" of the contact, and the coefficient of friction for this contact.<jupyter_code>import numpy as np
from multicontact_api import ContactType, ContactModel

cm_default = ContactModel()
print("Contact Type: ", cm_default.contact_type)
print("Friction coefficient: ", cm_default.mu)<jupyter_output>Contact Type:  CONTACT_UNDEFINED
Friction coefficient:  -1.0<jupyter_text>The default constructor create a `ContactModel` with "undefined" values. Other constructors exist to build a `ContactModel` with a friction coefficient or a contact type:<jupyter_code>cm1 = ContactModel(0.5)
print("cm1 friction coefficient:", cm1.mu)
print("cm1 Contact Type: ", cm1.contact_type)

cm2 = ContactModel(0.9, ContactType.CONTACT_POINT)
print("\ncm2 friction coefficient:", cm2.mu)
print("cm2 Contact Type: ", cm2.contact_type)
print("cm2 number of contact points: ", cm2.num_contact_points)
print("cm2 contact point position: ", cm2.contact_points_positions)<jupyter_output>cm1 friction coefficient: 0.5
cm1 Contact Type:  CONTACT_UNDEFINED

cm2 friction coefficient: 0.9
cm2 Contact Type:  CONTACT_POINT
cm2 number of contact points:  1
cm2 contact point position:  [0. 0. 0.]<jupyter_text>The contact point position are expressed in the contact frame (see the next section for more details). By default they are positionned at the origin. It is also possible to create contact model for planar contacts and define the contacts positions at the corners of a rectangular feet:<jupyter_code>cm_planar = ContactModel(0.5, ContactType.CONTACT_PLANAR)
# define 4 contacts points at the corners of a rectangle:
cm_planar.num_contact_points = 4

lx = 0.2 / 2. # half size of the feet along x axis
ly = 0.13 / 2. # half size of the feet along y axis

contact_points = np.zeros([3,4])
contact_points[0, :] = [-lx, -lx, lx, lx]
contact_points[1, :] = [-ly, ly, -ly, ly]

cm_planar.contact_points_positions = contact_points
# print the contact model data:

print("cm_planar: \n", cm_planar)<jupyter_output>cm_planar: 
 ContactType: 1, mu: 0.5
Number of contact points: 4, positions: 
  -0.1   -0.1    0.1    0.1
-0.065  0.065 -0.065  0.065
     0      0      0      0<jupyter_text>Contact PatchA contact patch define the placement (in SE(3), in the world frame) of a contact between a part of the robot and the environment. It contains a `ContactModel`.The main constructor take as input the placement as an SE(3) object from Pinocchio and a `ContactModel` as previously defined.<jupyter_code>from pinocchio import SE3, Quaternion
from multicontact_api import ContactPatch

placement = SE3()
placement.setRandom()

patch = ContactPatch(placement, cm_planar)

print("patch: \n", patch)<jupyter_output>patch: 
 Placement:
  R =
  0.740419  -0.428065   0.518208
  0.421023  -0.305616  -0.854013
  0.523946   0.850505 -0.0460591
  p = -0.563486 0.0258648  0.678224

ContactModel : ContactType: 1, mu: 0.5
Number of contact points: 4, positions: 
  -0.1   -0.1    0.1    0.1
-0.065  0.065 -0.065  0.065
     0      0      0      0<jupyter_text>The placement defined here is the placement of the origin of the 'contact frame'. As explained in the previous section the physical contact points positions may be different than this placement and they are expressed in this contact frame. In the previous example, the position of the front left corner of the feet, in the world frame is computed as follow:<jupyter_code>pos_RF = patch.placement * patch.contact_model.contact_points_positions[:, 3]
print(pos_RF)<jupyter_output>[ 0.27524087 -0.30677788  0.13904126]<jupyter_text>Other constructors exist to build a `ContactPatch` without requiring to build a `ContactModel`:<jupyter_code>p2 = SE3()
p2.setRandom()
patch2 = ContactPatch(p2, 0.5) # constructor with only a placement and a friction coefficient
print("patch2:\n", patch2)

p3 = SE3()
p3.setRandom()
patch3 = ContactPatch(p3) # constructor with only a placement
print("patch3:\n", patch3)<jupyter_output>patch2:
 Placement:
  R =
 -0.22528  0.817598 -0.529889
-0.394879  0.420576  0.816814
 0.890685  0.393254  0.228105
  p = 0.0485744 -0.012834   0.94555

ContactModel : ContactType: 0, mu: 0.5
Number of contact points: 1, positions: 
0
0
0


patch3:
 Placement:
  R =
  0.414966  -0.909316  0.0307914
  0.868596   0.406004    0.28408
  -0.27082 -0.0911383   0.958306
  p =  0.539828 -0.199543  0.783059

ContactModel : ContactType: 0, mu: -1
Number of contact points: 1, positions: 
0
0
0<jupyter_text>With these constructors, the `ContactModel` type is undefined and there is a single contact point at the origin of the frame.  Contact PhaseA contact phase is defined by a constant set of contact points. In the context of bipedal walking, two examples of contact phases are the single and double support phases.The most important member of the `ContactPhase` is thus a `map` defining the set of contact between a specific part of the robot and the environment.<jupyter_code>from multicontact_api import ContactPhase


cp = ContactPhase()
cp.addContact("right-feet", patch2)
cp.addContact("left-feet", patch3)<jupyter_output><empty_output><jupyter_text>The string argument choice is left to the user and is not required to match anything specifc. However, a good practice when working with a real robot model is to use the name of the frame of the robot creating the contact, as specified in the URDF.The `ContactPhase` class have several methods to access to the contacts already added:<jupyter_code>print("number of contacts: ", cp.numContacts()) 
print("effectors in contact: ", cp.effectorsInContact())

print("Is 'right-feet' in contact: ", cp.isEffectorInContact("right-feet"))
print("Is 'right-hand' in contact: ", cp.isEffectorInContact("right-hand"))

# Access the contact patch of a specific contact:
print("\n Contact placement of the right feet: \n", cp.contactPatch("right-feet").placement)<jupyter_output>number of contacts:  2
effectors in contact:  ['right-feet', 'left-feet']
Is 'right-feet' in contact:  True
Is 'right-hand' in contact:  False

 Contact placement of the right feet: 
   R =
 0.444451 -0.414554  0.794109
 0.175408 -0.829041 -0.530964
 0.878462  0.375281 -0.295751
  p =  0.257742 -0.270431 0.0268018<jupyter_text>The `map` can also be retrieved in Python as a Dictionary but this operation add a lot of overhead and should be avoided.<jupyter_code>dict_patches = cp.contactPatches()
print("effectors in contact: ", dict_patches.keys())<jupyter_output>effectors in contact:  dict_keys(['left-feet', 'right-feet'])<jupyter_text>The `ContactPhase` object also store different other optionnal data. For instance, a phase can be defined for a specific time interval:<jupyter_code>cp.timeInitial = 1.
cp.timeFinal = 3.5
print("Duration of the phase: ",cp.duration)<jupyter_output>Duration of the phase:  2.5<jupyter_text>Centroidal dataOptionnaly, a Contact Phase can store data related to the centroidal dynamic. It store the following initial and final values as public member:<jupyter_code># Initial values: the values at the beginning of the phase
cp.c_init = np.random.rand(3) # CoM position
cp.dc_init = np.random.rand(3) # CoM velocity
cp.ddc_init = np.random.rand(3) # CoM acceleration
cp.L_init = np.random.rand(3) # Angular momentum
cp.dL_init = np.random.rand(3) # Angular momentum time derivative
# Final values: the values at the end of the phase:
cp.c_final = np.random.rand(3)
cp.dc_final = np.random.rand(3)
cp.ddc_final = np.random.rand(3)
cp.L_final = np.random.rand(3)
cp.dL_final = np.random.rand(3)<jupyter_output><empty_output><jupyter_text>In addition to the initial and final values, a ContactPhase can store a trajectory for all of the following data:```Pythoncp.c_t  the CoM trajectory (dimension 3)cp.dc_t  the CoM velocity (dimension 3)cp.ddc_t  the CoM acceleration (dimension 3)cp.L_t   the Angular Momentum (dimension 3)cp.dL_t  the Angular Momentum time derivative (dimension 3)cp.wrench_t  the centroidal wrench (dimension 6)cp.zmp_t  the Zero Moment Point (dimension 3)cp.root_t  the base position and orientation (in SE3)```All of this trajectories are represented with objects from the NDCurves library (https://github.com/loco-3d/ndcurves). Example below add a third order polynomial trajectory as the CoM position:<jupyter_code>import ndcurves
from ndcurves import polynomial

coefs = np.random.rand(3, 4)  # generate random coefficients for the polynomial: 4 3D points
# build a 3rdf order polynomial of dimension 3 with random coefficients in a specific time interval:
c_t =  polynomial(coefs, cp.timeInitial, cp.timeFinal) 

cp.c_t = c_t

print("CoM position at t=2. : ", cp.c_t(2.))<jupyter_output>CoM position at t=2. :  [1.46198335 2.86437628 1.58724145]<jupyter_text>Whole body dataA Contact Phase can also store data related to the wholebody motion, it store the initial and final wholebody configuration:```Pythoncp.q_initcp.q_final```As long as the following trajectories:```Pythoncp.q_t  joint trajectorycp.dq_t  joint velocity trajectorycp.ddq_t  joint acceleration trajectorycp.tau_t  joint torque trajectory```  Effectors dataThe `ContactPhase` class can also store the contact forces and contact normal forces, in a `map` with the effector name as Key. In order to add a contact force trajectory for an effector, this effector must be in contact (see subsection above to add contact to the phase).<jupyter_code>coefs = np.random.rand(1, 4)  # generate random coefficients for the polynomial: 4 1D points
# build a 3rdf order polynomial of dimension 1 with random coefficients in a specific time interval:
normal_force =  polynomial(coefs, cp.timeInitial, cp.timeFinal) 
cp.addContactNormalForceTrajectory("right-feet", normal_force)

print("Contact normal force at t=2. :", cp.contactNormalForce("right-feet")(2.))
coefs = np.random.rand(12, 4)  # generate random coefficients for the polynomial: 4 12D points
forces = polynomial(coefs, cp.timeInitial, cp.timeFinal) 
cp.addContactForceTrajectory("right-feet", forces)<jupyter_output><empty_output><jupyter_text>The dimension of the trajectory for the contact forces is not fixed. It is up to the user to decide what is the representation of the forces stored. One of the main representation is to store the 3D force vector at each corner of the feet, leading to a curve of dimension 12. The `ContactModel` class provide the generator matrix that could be used to map this 12D contact forces to the 6D wrench expressed in the contact frame:<jupyter_code>contact_model = cp.contactPatch("right-feet").contact_model
contact_wrench = contact_model.generatorMatrix() @ cp.contactForce("right-feet")(2.)

print("Contact wrench for right-feet at t=2. : ", contact_wrench)<jupyter_output><empty_output><jupyter_text>Finally, the contact phase can also store effector trajectory for the swinging limbs in a `map` with the effector name as Key. The effector cannot be in contact and have an effector trajectory.<jupyter_code>from math import sqrt
from ndcurves import SE3Curve

# Build two SE3 for the initial/final placement of the feet:
init_pose = SE3.Identity()
end_pose = SE3.Identity()
init_pose.translation = np.array([0.2, -0.7, 0.6])
end_pose.translation = np.array([3.6, -2.2, -0.9])
init_pose.rotation = Quaternion.Identity().matrix()
end_pose.rotation = Quaternion(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0).normalized().matrix()
# Build a curve in SE3 doing a linear interpolation between this two placements:
eff_HL = SE3Curve(init_pose, end_pose, cp.timeInitial,cp.timeFinal)

cp.addEffectorTrajectory("left-hand",eff_HL)
print("Left hand placement at t=2.5: \n", cp.effectorTrajectory("left-hand").evaluateAsSE3(2.5))<jupyter_output>Left hand placement at t=2.5: 
   R =
        1         0         0
        0  0.587785 -0.809017
        0  0.809017  0.587785
  p = 2.24 -1.6 -0.3<jupyter_text>Contact SequenceAs soon as a creation or a rupture of contact point occurs the contact set is modified, defining a new contact phase.The concatenation of contact phases describes what we name a contact sequence, inside which all the contact phases have their own duration.A contact sequence is basically a Vector of Contact Phase, with several helper method which can be used to ease the creation of a Contact Sequence. One can either create a Contact sequence with a know number of contact Phase and correctly set the members of the Contact Phases with:<jupyter_code>from multicontact_api import ContactSequence
cs = ContactSequence(3)

cs.contactPhases[0] = cp # assign the phase defined below as the first one
print("CoM position at the beginning of the first phase: ", cs.contactPhases[0].c_init)

cp1 = cs.contactPhases[1]
cp1.c_final = np.array([1., 0. ,0.85])
print("CoM position at the end of the second phase: ", cs.contactPhases[1].c_final)<jupyter_output>CoM position at the beginning of the first phase:  [0.21871697 0.86272939 0.97332564]
CoM position at the end of the second phase:  [1.   0.   0.85]<jupyter_text>Or simply append new Contact Phase at the end of the current Contact Sequence:<jupyter_code>cp3 = ContactPhase()
cp3.c_final = np.array([5, 0, 0.85])

cs.append(cp3)
print("CoM position at the end of the last phase: ", cs.contactPhases[-1].c_final)<jupyter_output>CoM position at the end of the last phase:  [5.   0.   0.85]
{'fraction_non_alphanumeric': 0.08210653104925053, 'fraction_numerical': 0.048782119914346896, 'mean_word_length': 4.772499034376207, 'pattern_counts': {'":': 0, '<': 56, '<?xml version=': 0, '>': 56, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '17', 'dir': 'jupyter-structured-clean-dedup', 'id': '453821', 'license': 'BSD-2-Clause', 'n_tokens_mistral': 4966, 'n_tokens_neox': 4501, 'n_words': 1823, 'path': 'notebooks/1_basic_usage.ipynb', 'repo_name': 'nim65s/multicontact-api', 'size': '14944'}