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)
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])
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])
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
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
def __init__(self, robot):
self.robot = robot
lambdas.setRobotArgs(robot)
def __init__(self,robot):
self.robot = robot
lambdas.setRobotArgs(robot)
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)