def update(self, rstate, dt):
"""
"""
CH = convex_hull(extract_contact_points(self.frames, self.dof))
if self.point_name in rstate:
point = rstate[self.point_name]
elif self.point_name == 'CoM':
point = com_properties(self.LQP_ctrl.bodies, compute_J=False)
rstate['CoM'] = point
if len(point) == 3:
point = point[self.dof]
cond_validity = is_in_convex_hull(CH, point, self.margin)
return cond_validity
def update(self, rstate, dt):
"""
"""
CH = convex_hull(extract_contact_points(self.frames, self.dof))
if self.point_name in rstate:
point = rstate[self.point_name]
elif self.point_name == 'CoM':
point = com_properties(self.LQP_ctrl.bodies, compute_J=False)
rstate['CoM'] = point
if len(point) == 3:
point = point[self.dof]
cond_validity = is_in_convex_hull(CH, point, self.margin)
return cond_validity
def _update_matrices(self, rstate, dt):
"""
"""
if "Pcom" not in rstate:
com_gpos, J, dJ = com_properties(self.lqp_bodies)
com_gvel = dot(J, rstate['gvel'])
rstate['Pcom'] = com_gpos
rstate['Vcom'] = com_gvel
rstate['Jcom'] = J
rstate['dJcom'] = dJ
cmd = self._ctrl.update(rstate['Pcom'], rstate['Vcom'], rstate, dt)
self._J[:] = rstate['Jcom'][self._cdof, :]
self._dJ[:] = rstate['dJcom'][self._cdof, :]
self._dVdes[:] = cmd[self._cdof]
