Advertisement
Guest User

Untitled

a guest
Jan 15th, 2019
143
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 1.09 KB | None | 0 0
  1.     def get_body_jacp(self, name, np.ndarray[double, ndim=1, mode="c"] jacp = None):
  2.         id = self._model.body_name2id(name)
  3.         if jacp is None:
  4.             jacp = np.zeros(3 * self._model.nv)
  5.         cdef double * jacp_view = &jacp[0]
  6.         mj_jacBody(self._model.ptr, self.ptr, jacp_view, NULL, id)
  7.         return jacp
  8.  
  9.     def get_body_jacr(self, name, np.ndarray[double, ndim=1, mode="c"] jacr = None):
  10.         id = self._model.body_name2id(name)
  11.         if jacr is None:
  12.             jacr = np.zeros(3 * self._model.nv)
  13.         cdef double * jacr_view = &jacr[0]
  14.         mj_jacBody(self._model.ptr, self.ptr, NULL, jacr_view, id)
  15.         return jacr
  16.  
  17.     def get_body_xvelp(self, name):
  18.         id = self._model.body_name2id(name)
  19.         jacp = self.get_body_jacp(name).reshape((3, self._model.nv))
  20.         xvelp = np.dot(jacp, self.qvel)
  21.         return xvelp
  22.  
  23.     def get_body_xvelr(self, name):
  24.         id = self._model.body_name2id(name)
  25.         jacr = self.get_body_jacr(name).reshape((3, self._model.nv))
  26.         xvelr = np.dot(jacr, self.qvel)
  27.         return xvelr
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement