Example #1
0
 def __init__(self, robot):
     self.robot = robot
     lambdas.setRobotArgs(robot)
     self.YJ = zero([6, robot.model.nv])
     self.Q = np.zeros([
         robot.model.nv,
     ] * 3)
Example #2
0
 def __init__(self, robot):
     self.robot = robot
     lambdas.setRobotArgs(robot)
     NV = robot.model.nv
     NJ = robot.model.njoints
     self.C = zero([NV, NV])
     self.YS = zero([6, NV])
     self.Sxv = zero([6, NV])
Example #3
0
 def __init__(self,robot):
     self.robot = robot
     lambdas.setRobotArgs(robot)
     NV = robot.model.nv
     NJ = robot.model.njoints
     self.C = zero([NV,NV])
     self.YS = zero([6,NV])
     self.Sxv = zero([6,NV])
Example #4
0
def hessian(robot, q, crossterms=False):
    '''
    Compute the Hessian tensor of all the robot joints.
    If crossterms, then also compute the Si x Si terms, 
    that are not part of the true Hessian but enters in some computations like VRNEA.
    '''
    lambdas.setRobotArgs(robot)
    H = np.zeros([6, robot.model.nv, robot.model.nv])
    se3.computeJointJacobians(robot.model, robot.data, q)
    J = robot.data.J
    skiplast = -1 if not crossterms else None
    for joint_i in range(1, robot.model.njoints):
        for joint_j in ancestors(joint_i)[:skiplast]:  # j is a child of i
            for i in iv(joint_i):
                for j in iv(joint_j):
                    Si = J[:, i]
                    Sj = J[:, j]
                    H[:, i, j] = np.asarray(Mcross(Sj, Si))[:, 0]
    return H
Example #5
0
def hessian(robot,q,crossterms=False):
    '''
    Compute the Hessian tensor of all the robot joints.
    If crossterms, then also compute the Si x Si terms, 
    that are not part of the true Hessian but enters in some computations like VRNEA.
    '''
    lambdas.setRobotArgs(robot)
    H=np.zeros([6,robot.model.nv,robot.model.nv])
    pin.computeJointJacobians(robot.model,robot.data,q)
    J = robot.data.J
    skiplast = -1 if not crossterms else None
    for joint_i in range(1,robot.model.njoints):
        for joint_j in ancestors(joint_i)[:skiplast]: # j is a child of i
            for i in iv(joint_i):
                for j in iv(joint_j):
                    Si  = J[:,i]
                    Sj  = J[:,j]
                    H[:,i,j] = np.asarray(Mcross(Sj,    Si))[:,0]
    return H
Example #6
0
 def __init__(self, robot):
     self.robot = robot
     lambdas.setRobotArgs(robot)
Example #7
0
 def __init__(self,robot):
     self.robot = robot
     lambdas.setRobotArgs(robot)
Example #8
0
 def __init__(self,robot):
     self.robot = robot
     lambdas.setRobotArgs(robot)
     self.YJ = zero([6,robot.model.nv])
     self.Q  = np.zeros([robot.model.nv,]*3)