Unverified Commit c27d5c87 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #463 from proyan/topic/robotwrapper

[Python] Minor. Remove duplicate methods. minor rearrangement
parents f0d33c6c 3a0cac71
Pipeline #903 failed with stages
in 9 seconds
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment