Code refactoring

[andreadelprete]

Hi @hammoudbilal

as we already started discussing, the code could probably use some refactoring because at the moment it's a bit messy. I haven't had the time to think about it properly, but I have recently developed a simple simulator in Python for a class I am teaching. I've tried to make it as simple as possible so that student could understand it easily. I'm sharing it with you here. Maybe this code could be a starting point to think of the new code

import pinocchio as se3
import numpy as np
from numpy.linalg import norm
import os
import math
import time

class ContactPoint:
    ''' A point on the robot surface that can make contact with surfaces.
    '''
    def __init__(self, model, data, frame_name):
        self.model = model      # robot model
        self.data = data        # robot data
        self.frame_name = frame_name    # name of reference frame associated to this contact point
        self.frame_id = model.getFrameId(frame_name)    # id of the reference frame
        self.active = False         # True if this contact point is in contact

    def get_position(self):
        ''' Get the current position of this contact point 
        '''
        M = self.data.oMf[self.frame_id]
        return M.translation

    def get_velocity(self):
        M = self.data.oMf[self.frame_id]
        R = se3.SE3(M.rotation, 0*M.translation)    # same as M but with translation set to zero
        v_local = se3.getFrameVelocity(self.model, self.data, self.frame_id)
        v_world = (R.act(v_local)).linear   # convert velocity from local frame to world frame
        return v_world

    def get_jacobian(self):
        J6 = se3.getFrameJacobian(self.model, self.data, self.frame_id, se3.ReferenceFrame.LOCAL_WORLD_ALIGNED)
        return J6[:3,:]

class ContactSurface:
    ''' A visco-elastic planar surface
    '''
    def __init__(self, name, pos, normal, K, B, mu):
        self.name = name        # name of this contact surface
        self.x0 = pos           # position of a point of the surface
        self.normal = normal    # direction of the normal to the surface
        self.K = K              # stiffness of the surface material
        self.B = B              # damping of the surface material
        self.mu = mu            # friction coefficient of the surface
        self.bias = self.x0.dot(self.normal)

    def check_collision(self, p):
        ''' Check the collision of the given point
            with this contact surface. If the point is not
            inside this surface, then return False.
        '''
        normal_penetration = self.bias - p.dot(self.normal)
        if(normal_penetration < 0.0):
            return False # no penetration
        return True

    def compute_force(self, contact_point, anchor_point):
        cp = contact_point
        p0 = anchor_point
        p = cp.get_position()
        v = cp.get_velocity()

        # compute contact force using spring-damper law
        f = self.K.dot(p0 - p) - self.B.dot(v)

        # check whether contact force is outside friction cone
        f_N = f.dot(self.normal)   # norm of normal force
        f_T = f - f_N*self.normal  # tangential force (3d)
        f_T_norm = norm(f_T)            # norm of tangential force
        if(f_T_norm > self.mu*f_N):
            # contact is slipping 
            t_dir = f_T / f_T_norm  # direction of tangential force
            # saturate force at the friction cone boundary
            f = f_N*self.normal + self.mu*f_N*t_dir

            # update anchor point so that f is inside friction cone
            delta_p0 = (f_T_norm - self.mu*f_N) / self.K[0,0]
            p0 -= t_dir*delta_p0

        return f, p0

class Contact:
    ''' A contact between a contact-point and a contact-surface
    '''
    def __init__(self, contact_point, contact_surface):
        self.cp = contact_point
        self.cs = contact_surface
        self.reset_contact_position()

    def reset_contact_position(self):
        # Initialize anchor point p0, that is the initial (0-load) position of the spring
        self.p0 = self.cp.get_position()
        self.in_contact = True

    def compute_force(self):
        self.f, self.p0 = self.cs.compute_force(self.cp, self.p0)
        return self.f

    def get_jacobian(self):
        return self.cp.get_jacobian()

class RobotSimulator:

    # Class constructor
    def __init__(self, conf, robot):
        self.conf = conf
        self.robot = robot
        self.model = self.robot.model
        self.data = self.model.createData()
        self.t = 0.0                    # time
        self.nv = nv = self.model.nv    # Dimension of joint velocities vector
        self.na = na = robot.na         # number of actuated joints
        # Matrix S used as filter of vetor of inputs U
        self.S = np.hstack((np.zeros((na, nv-na)), np.eye(na, na)))

        self.contacts = []
        self.candidate_contact_points = [] # candidate contact points
        self.contact_surfaces = []

        self.init(conf.q0, None, True)

        self.tau_c = np.zeros(na)   # Coulomb friction torque
        self.simulate_coulomb_friction = conf.simulate_coulomb_friction
        self.simulation_type = conf.simulation_type
        if(self.simulate_coulomb_friction):
            self.tau_coulomb_max = 1e-2*conf.tau_coulomb_max*self.model.effortLimit
        else:
            self.tau_coulomb_max = np.zeros(na)

    # Re-initialize the simulator
    def init(self, q0=None, v0=None, reset_contact_positions=False):
        self.first_iter = True

        if q0 is not None:
            self.q = q0.copy()

        if(v0 is None):
            self.v = np.zeros(self.robot.nv)
        else:
            self.v = v0.copy()
        self.dv = np.zeros(self.robot.nv)
        self.resize_contact_data(reset_contact_positions)

    def resize_contact_data(self, reset_contact_positions=False):
        self.nc = len(self.contacts)                    # number of contacts
        self.nk = 3*self.nc                             # size of contact force vector
        self.f = np.zeros(self.nk)                      # contact forces
        self.Jc = np.zeros((self.nk, self.model.nv))    # contact Jacobian

        # reset contact position
        if(reset_contact_positions):
            se3.forwardKinematics(self.model, self.data, self.q)
            se3.updateFramePlacements(self.model, self.data)
            for c in self.contacts:
                c.reset_contact_position()

        self.compute_forces(compute_data=True)

    def add_candidate_contact_point(self, frame_name):
        self.candidate_contact_points += [ContactPoint(self.model, self.data, frame_name)]

    def add_contact_surface(self, name, pos, normal, K, B, mu):
        ''' Add a contact surface (i.e., a wall) located at "pos", with normal 
            outgoing direction "normal", 3d stiffness K, 3d damping B.
        '''
        self.contact_surfaces += [ContactSurface(name, pos, normal, K, B, mu)]

    def collision_detection(self):
        for s in self.contact_surfaces:     # for each contact surface
            for cp in self.candidate_contact_points: # for each candidate contact point
                p = cp.get_position()
                if(s.check_collision(p)):   # check whether the point is colliding with the surface
                    if(not cp.active): # if the contact was not already active
                        print("Collision detected between point", cp.frame_name, " at ", p)
                        cp.active = True
                        cp.contact = Contact(cp, s)
                        self.contacts += [cp.contact]
                        self.resize_contact_data()
                else:
                    if(cp.active):
                        print("Contact lost between point", cp.frame_name, " at ", p)
                        cp.active = False
                        self.contacts.remove(cp.contact)
                        self.resize_contact_data()

    def compute_forces(self, compute_data=True):
        '''Compute the contact forces from q, v and elastic model'''
        if compute_data:
            se3.forwardKinematics(self.model, self.data, self.q, self.v)
            # Computes the placements of all the operational frames according to the current joint placement stored in data
            se3.updateFramePlacements(self.model, self.data)
            self.collision_detection()

        i = 0
        for c in self.contacts:
            self.f[i:i+3] = c.compute_force()
            self.Jc[i:i+3, :] = c.get_jacobian()
            i += 3
        return self.f

    def step(self, u, dt):
        # (Forces are directly in the world frame, and aba wants them in the end effector frame)
        se3.computeAllTerms(self.model, self.data, self.q, self.v)
        se3.updateFramePlacements(self.model, self.data)
        M = self.data.M
        h = self.data.nle
        self.collision_detection()
        self.compute_forces(False)

        self.tau_c = self.tau_coulomb_max*np.sign(self.v[-self.na:])
        self.dv = np.linalg.solve(M, self.S.T.dot(u-self.tau_c) - h + self.Jc.T.dot(self.f))
        v_mean = self.v + 0.5*dt*self.dv
        self.v += self.dv*dt
        self.q = se3.integrate(self.model, self.q, v_mean*dt)

        self.t += dt
        return self.q, self.v

    def reset(self):
        self.first_iter = True

    def simulate(self, u, dt=0.001, ndt=1):
        ''' Perform ndt steps, each lasting dt/ndt seconds '''
        tau_c_avg = 0*self.tau_c
        for i in range(ndt):
            self.q, self.v = self.step(u, dt/ndt)
            tau_c_avg += self.tau_c
        self.tau_c = tau_c_avg / ndt

        return self.q, self.v, self.f

If you prefer I can push it in the repo, together with a simple test and the additional code for visualization with gepetto-viewer. I don't know if all of this is necessary though if we wanna use it just as inspiration for the design.

[andreadelprete]

Hi @hammoudbilal

I like the idea of getting rid of the Contact class and moving the anchor point inside ContactPoint. Most of the problems with the new code are with the Simulator classes. Maybe it's because you're trying to share too much code and structure between the Euler and Exponential simulator, which are too different in terms of implementation to share that much code.

I think that this should roughly be the logic flow of a simulation step:

compute all kinematics-dynamic terms based on current state (q,v)
for cp in ContactPointList: 
    cp.update_pos_vel()
    for co in ObjectList: 
        collision = co.check_collision(cp)
        # check_collision updates normal-tangential directions stored in cp

Then for the EULER SIMULATOR we should have:

tau_contact_forces.setZero()
for cp in ContactPointList: 
    if cp.active: 
        cp.object.contact_model.computeForce(cp)
        # the computed contact force is stored in cp
        tau_contact_forces += cp.get_tau_contact_forces()

While for the EXPONENTIAL SIMULATOR:

for cp in ContactPointList: 
    if cp.active: 
        fill matrices J, dJv, p0, p, dp, Kpo
Compute A and b
Compute tau_contact_forces via matrix exponential
Saturate forces at friction cones
Update anchor points

And finally both simulators should finish with

Compute joint accelerations based on tau_contact_forces

With this idea in mind, these are some issues I've found with the code of your PR.

• checkContact should rather be called detectContacts.
• If computeContactState and computeContactForces are always called one after the other, then it should be just one method.
• In ExponentialSimulator, the method computeContactForces doesn't really compute the contact forces but it just computes the quantities needed for computing later the contact forces in step. This is really confusing!
• In both simulators, the order of operations inside step is weird and should be reversed, i.e. first collision detection, then compute forces, then compute accelerations and integrate
• In general the names of the methods are often misleading which make the code hard to read and understand

[hammoudbilal]

thanks for the tips, I will clean it up

[hammoudbilal]

Hi @andreadelprete

there are few issues remaining

• computeContactForces for EulerSimulator is done at the end of the integration loop for convenience, this way when reading state x{t+1} at the end of the step, the code also returns the current forces f{t+1} instead of ft at the beginning of the step. for ExponentialSimulator, computeContactForces could be called right after computing the integral intxt to return f{t+1} then the joint accelerations/velocities and positions could be updated. (maybe i'm wrong about this?)

• updating the anchor points through the qp is still missing, this function is supposed to handle it, but I avoided pushing it with this pull request #11 . However I wrote down the QP for n contact points all together in the pdf file below. The QP optimizes for dp_0, then the delta_p0 could be integrated. Can you please check if what I wrote makes sense? looking at the constraints, the matrices are block diagonal, it seems that the only important factor is the matrix I defined as \Bar{A} anchor_point_qp.pdf I am guessing I could exploit this and re-write checkFrictionCone based on on the transformation that comes from the corresponding block in \Bar{A}? maybe I could write the QP for each contact point independently ?

  • I spent sometime trying to understand why the trajectories of ExponentialSimulator differ a bit from EulerSimulator, this was first discussed in #1 . Following your code, you compute

dJv_local_ = alocal_ + w.cross(v); dJv_ = Adjoint * dJv_local_;

in order to get 'alocal_' the second order pinocchio::forwardKinematics must be called. I get a segmentation fault the case where i use

pinocchio::computeAllTerms(); pinocchio::updateFramePlacements(); pinocchio:: forwardKinematics();

if I do the opposite, alocal_ becomes zero

pinocchio:: forwardKinematics(); pinocchio::computeAllTerms(); pinocchio::updateFramePlacements();

I will have to dig deeper into this, I still don't have an answer.

[andreadelprete]

* `computeContactForces` for `EulerSimulator` is done at the end of the integration loop for convenience, this way when reading state x_{t+1} at the end of the step, the code also returns the current forces f_{t+1} instead of f_t at the beginning of the step. 

I see. Let's leave it like this for now, but let's make this consistent with the exponential simulator.

for ExponentialSimulator, computeContactForces could be called right after computing the integral intxt to return f_{t+1} then the joint accelerations/velocities and positions could be updated. (maybe i'm wrong about this?)

The force computed using int_xt is a sort of "predicted force" obtained via the linear dynamical system. This is gonna be different from the real force you get from the nonlinear system after updating q and v. Beware of this difference. I think that from python we wanna be able to access both these forces to analyze what's going on inside the simulator. If the time step we use is not too large, then the two forces should be rather close.

I wrote down the QP for n contact points all together in the pdf file below. The QP optimizes for dp_0, then the delta_p0 could be integrated.  Can you please check if what I wrote makes sense? 

At a first glance it all makes sense. I'll take a better look later.

looking at the constraints, the matrices are block diagonal, it seems that the only important factor is the matrix I defined as \Bar{A} anchor_point_qp.pdf I am guessing I could exploit this and re-write checkFrictionCone based on on the transformation that comes from the corresponding block in \Bar{A}? maybe I could write the QP for each contact point independently ?

I think the constraint matrix in the QP is dense, because it contains the matrix exponential, which is a dense matrix, so unfortunately you cannot solve a separate QP for each contact (but please double check as I may be wrong).

* I spent sometime trying to understand why the trajectories of `ExponentialSimulator`  differ a bit from `EulerSimulator`, this was first discussed in #1 . Following your code, you compute

dJv_local_ = alocal_ + w.cross(v); dJv_ = Adjoint * dJv_local_;

in order to get 'alocal_' the second order pinocchio::forwardKinematics must be called. I get a segmentation fault the case where i use

pinocchio::computeAllTerms(); pinocchio::updateFramePlacements(); pinocchio:: forwardKinematics();

if I do the opposite, alocal_ becomes zero

pinocchio:: forwardKinematics(); pinocchio::computeAllTerms(); pinocchio::updateFramePlacements();

I will have to dig deeper into this, I still don't have an answer.

Ok, I'll also take a look to see if I can help with this. At the moment, this doesn't ring a bell.

[andreadelprete]

Regarding the QP (Eq. 28 in the paper), you don't need the constraint on the normal forces being positive because the other 4 constraints on the tangential forces, when put together, automatically admit only solutions with positive normal forces.

[andreadelprete]

Regarding the computation of pinocchio, computeAllTerms is equivalent to calling:

            pinocchio::forwardKinematics
            pinocchio::crba
            pinocchio::nonLinearEffects
            pinocchio::computeJointJacobians
            pinocchio::centerOfMass
            pinocchio::jacobianCenterOfMass
            pinocchio::kineticEnergy
            pinocchio::potentialEnergy

This is too much for our needs, so we call only the functions we need, including those for the frame kinematics and the computation of dJv:

        pin.forwardKinematics(self.model, self.data, q, v, np.zeros(self.model.nv))
        pin.computeJointJacobians(self.model, self.data)
        pin.updateFramePlacements(self.model, self.data)
        pin.crba(self.model, self.data, q)
        pin.nonLinearEffects(self.model, self.data, q, v)

[hammoudbilal]

Computing dJv_local_ = alocal_ + w.cross(v) when computing alocal_ causes the simulation to diverge. As a sanity check I switched to computing dJv_local_ = dJdt_local_ * v_ ;

this method produces results similar to computing dJv_local_ = w.cross(v) where the simulation converges with some error relative to the Euler Simulator , this I still find a bit strange, maybe we should move this to a separate issue.

Also ExponentialSimulator::checkFrictionCone() now updates f_projected = f_average in all cases assuming that ExponentialSimulator::computeSlipping() will have to update it, I would rather add the qp implementation in a different pull request though.

let me know if I need to address more refactoring issues before merging this pull request

[andreadelprete]

This issue seems to be solved now, including the computation of dJv (how did you solve it by the way?), so I think we can close this.

[hammoudbilal]

i switched back to second order forward kinematics with zero acceleration. Yes it is solved, I will close it