Closed grizzi closed 4 years ago
Nice code @grizzi ... and what is your issue?
The arm is unstable and does not reach the end effector reference x_des
which is in proximity of the current one (which I visually check through pose markers in RViz). I am concerned I am misunderstanding something.
By the way @jcarpent , thanks for the super fast reply!
I think your Jacobians are not expressed in the same frame than your errors. In the example section, there is a code for doing the inverse kinematics. I would suggest starting from it ;)
I tought this was expressed through the enum pinocchio::WORLD
but I will check that again!
pinocchio::Motion xd = getFrameVelocity(model_, data_, frame_id_);
is expressed in the LOCAL frame of the Frame
@grizzi I hope the issue is fixed on your side. I will close it. Feel free to reopen it if needed.
Hi and thanks again for your work @jcarpent . I have jumped back on this work and after reading about spatial algebra things are clearing up.
From my understanding, the usual Euclidean base referenced frame is expressed through LOCAL_WORLD_ALIGNED
. To derive my task space controller I still need the frameJacobianTimeVariation
in such frame, but I cannot get it correct.
In issue a soution is provided for the joint jacobian derivative. I am using pythoon to prototype and while finite differences work (compared to hand derived model), the pinocchio method provides zero output:
pin.computeJointJacobians(self.model, self.data, self.q)
pin.framesForwardKinematics(self.model, self.data, self.q)
pin.updateFramePlacements(self.model, self.data)
pin.computeJointJacobiansTimeVariation(self.model, self.data, self.q, self.v)
dJ = pin.getFrameJacobianTimeVariation(self.model, self.data, self.ee_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
# dJ = np.matrix(np.zeros(6,self.model.nv))
Looking in the code, it seems that the function getFrameJacobianTimeVariation
skips the case rf==LOCAL_WORLD_ALIGNED
which I guess it is why the output.
Is this intended (possibly it would be good to get some not implemented warning here)? Could you provide an hint on how to get this quantity in the LOCAL_WORLD_ALIGNED
frame?
Best, Giuseppe
OK. I may implement it in the next few days. Is it fine for you?
Sure, and thanks for the effort, I really apprecciate! For the moment, I am using finite differences.
Solved by #1220.
Hi, I have been trying to write a task space controller with no success so far. If it would work, I could add it to the list of examples, since this seems to be missing.
I cannot understand what is going wrong with the code. I have been using
pinocchio
also to simulate forward the robot. Here is the main part of the code (assume variables get initialized where needed):I was using RViz to visualize the robot since I plan to use this tool together with ROS. I would like to know if there is something wrong in the implementation. Thanks for the support!