diff --git a/bindings/python/scripts/robot_wrapper.py b/bindings/python/scripts/robot_wrapper.py index 8738a43c0207a1294f87bdd70f2dda8d93a95b23..28b8058b8915591eb78e7cd633085c84f07e720f 100644 --- a/bindings/python/scripts/robot_wrapper.py +++ b/bindings/python/scripts/robot_wrapper.py @@ -110,6 +110,11 @@ class RobotWrapper(object): se3.forwardKinematics(self.model, self.data, q, v) return self.data.v[index] + def acceleration(self, q, v, a, index, update_kinematics=True): + if update_kinematics: + se3.forwardKinematics(self.model, self.data, q, v, a) + return self.data.a[index] + def framePosition(self, q, index, update_kinematics=True): if update_kinematics: se3.forwardKinematics(self.model, self.data, q) @@ -124,11 +129,6 @@ class RobotWrapper(object): parentJointVel = self.data.v[frame.parent] return frame.placement.actInv(parentJointVel) - def acceleration(self, q, v, a, index, update_kinematics=True): - if update_kinematics: - se3.forwardKinematics(self.model, self.data, q, v, a) - return self.data.a[index] - def frameAcceleration(self, q, v, a, index, update_kinematics=True): if update_kinematics: se3.forwardKinematics(self.model, self.data, q, v, a) @@ -136,6 +136,15 @@ class RobotWrapper(object): parentJointAcc = self.data.a[frame.parent] return frame.placement.actInv(parentJointAcc) + def frameClassicAcceleration(self, q, v, a, index, update_kinematics=True): + if update_kinematics: + se3.forwardKinematics(self.model, self.data, q, v, a) + f = self.model.frames[index] + af = f.placement.actInv(self.data.a[f.parent]) + vf = f.placement.actInv(self.data.v[f.parent]) + af.linear += np.cross(vf.angular.T, vf.linear.T).T + return af; + def jacobian(self, q, index, update_kinematics=True, local_frame=True): return se3.jacobian(self.model, self.data, q, index, local_frame, update_kinematics) @@ -158,27 +167,7 @@ class RobotWrapper(object): def framesKinematics(self, q): se3.framesKinematics(self.model, self.data, q) - - def framePosition(self, index): - f = self.model.frames[index] - return self.data.oMi[f.parent].act(f.placement) - - def frameVelocity(self, index): - f = self.model.frames[index] - return f.placement.actInv(self.data.v[f.parent]) - - ''' Return the spatial acceleration of the specified frame. ''' - def frameAcceleration(self, index): - f = self.model.frames[index] - return f.placement.actInv(self.data.a[f.parent]) - - def frameClassicAcceleration(self, index): - f = self.model.frames[index] - a = f.placement.actInv(self.data.a[f.parent]) - v = f.placement.actInv(self.data.v[f.parent]) - a.linear += np.cross(v.angular.T, v.linear.T).T - return a; - + ''' Call computeJacobians if update_geometry is true. If not, user should call computeJacobians first. Then call getJacobian and return the resulted jacobian matrix. Attention: if update_geometry is true, the function computes all the jacobians of the model. It is therefore outrageously costly wrt a