def compute_wrench_face(self, p):
"""
Compute the face matrix of the contact wrench cone in the world frame.
INPUT:
- ``p`` -- point where the resultant wrench is taken at
OUTPUT:
The friction matrix F(p) such that all valid contact wrenches satisfy:
F(p) * w(p) <= 0,
where w(p) is the resultant contact wrench at p.
"""
S = self.compute_wrench_span(p)
return Cone.face_of_span(S)
def compute_wrench_face(self, p):
"""
Compute the face matrix of the contact wrench cone in the world frame.
Parameters
----------
p : array, shape=(3,)
Point where the resultant wrench is taken at.
Returns
-------
F : array, shape=(m, 6)
Friction matrix such that all valid contact wrenches satisfy
:math:`F w \\leq 0`, where `w` is the resultant contact wrench at
`p`.
"""
S = self.compute_wrench_span(p)
return Cone.face_of_span(S)
def compute_wrench_cone(self, p):
"""
Compute the face matrix of the contact wrench cone in the world frame.
INPUT:
- ``p`` -- point where the resultant wrench is taken at
OUTPUT:
The friction matrix F(p) such that all valid contact wrenches satisfy:
F(p) * w(p) <= 0,
where w(p) is the resultant contact wrench at p.
"""
# F = self.compute_stacked_wrench_cones()
# G = self.compute_grasp_matrix(p)
# S0 = Cone.span_of_face(F)
# S = dot(-G, S0)
S = self.compute_wrench_span(p)
return Cone.face_of_span(S)
def wrench_cone(self):
"""
Contact wrench friction cone (CWC).
"""
return Cone(face=self.wrench_face, rays=self.wrench_rays)
def force_cone(self):
"""
Contact force friction cone.
"""
return Cone(face=self.force_face, rays=self.force_rays)
