Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- def get_body_jacp(self, name, np.ndarray[double, ndim=1, mode="c"] jacp = None):
- id = self._model.body_name2id(name)
- if jacp is None:
- jacp = np.zeros(3 * self._model.nv)
- cdef double * jacp_view = &jacp[0]
- mj_jacBody(self._model.ptr, self.ptr, jacp_view, NULL, id)
- return jacp
- def get_body_jacr(self, name, np.ndarray[double, ndim=1, mode="c"] jacr = None):
- id = self._model.body_name2id(name)
- if jacr is None:
- jacr = np.zeros(3 * self._model.nv)
- cdef double * jacr_view = &jacr[0]
- mj_jacBody(self._model.ptr, self.ptr, NULL, jacr_view, id)
- return jacr
- def get_body_xvelp(self, name):
- id = self._model.body_name2id(name)
- jacp = self.get_body_jacp(name).reshape((3, self._model.nv))
- xvelp = np.dot(jacp, self.qvel)
- return xvelp
- def get_body_xvelr(self, name):
- id = self._model.body_name2id(name)
- jacr = self.get_body_jacr(name).reshape((3, self._model.nv))
- xvelr = np.dot(jacr, self.qvel)
- return xvelr
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement