Task space controller robotic arm

[grizzi]

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):

pin::urdf::buildModel(urdf_path, model_, true);
pin::Data data_(model_);

q_ = pinocchio::neutral(model_);
dq_ = Eigen::VectorXd::Zero(model_.nv);
ddq_ = Eigen::VectorXd::Zero(model_.nv);
tau_ = Eigen::VectorXd::Zero(model_.nv);

// desired end effector pose near the pose where the ee gets initialized
pinocchio::SE3 x_des = pinocchio::SE3(x_des_R, x_des_t);

// gains
kp_ = Eigen::Matrix<double, 6, 6>::Identity()*10.0;
kd_ = Eigen::Matrix<double, 6, 6>::Identity()*1.0;

// repeat the following in a loop

// compute task space error
const pinocchio::SE3 dMi = x_des.actInv(data_.oMf[frame_id_]);
Eigen::VectorXd dx = pinocchio::log6(dMi).toVector();
pinocchio::Motion xd = getFrameVelocity(model_, data_, frame_id_);
Eigen::VectorXd dxd = xd.toVector();
xdd_des_ = kp_ * dx - kd_ * dxd;

// update jacobians
pin::computeJointJacobians(model_, data_, q_);       // this also compute the forward kinematics for joints
pin::framesForwardKinematics(model_, data_, q_);     // this update the frames placement
pin::getFrameJacobian(model_, data_, frame_id_, pin::WORLD, J_);
pin::computeJointJacobiansTimeVariation(model_, data_, q_, dq_);
pin::getFrameJacobianTimeVariation(model_, data_, frame_id_, pinocchio::WORLD, dJ_);

// compute feedforward torque
// I wrote the function to compute the pseudoinverse based on SVD 
Eigen::VectorXd qdd_des = math_tools::computePseudoinverse(J_) * (xdd_des_ - dJ_ * dq_);
pin::computeAllTerms(model_, data_, q_, dq_);

// get mass matrix
pin::crba(model_, data_, q_);
data_->M.triangularView<Eigen::StrictlyLower>() = data_->M.transpose().template triangularView<Eigen::StrictlyLower>();
tau_ = data_.M*qdd_des +  data_.nle - 0.1*dq_; // add some small friction

// advance dynamics 
ddq_ = pin::aba(model_, data_, q_, dq_, tau_);
dq_ += ddq_ * dt;
q_ = pin::integrate(*model_, q_, dq_ * dt);

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!

[jcarpent]

Nice code @grizzi ... and what is your issue?

[grizzi]

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.

[grizzi]

By the way @jcarpent , thanks for the super fast reply!

[jcarpent]

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 ;)

[grizzi]

I tought this was expressed through the enum pinocchio::WORLD but I will check that again!

[jcarpent]

pinocchio::Motion xd = getFrameVelocity(model_, data_, frame_id_); is expressed in the LOCAL frame of the Frame

[jcarpent]

@grizzi I hope the issue is fixed on your side. I will close it. Feel free to reopen it if needed.

[grizzi]

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

[jcarpent]

OK. I may implement it in the next few days. Is it fine for you?

[grizzi]

Sure, and thanks for the effort, I really apprecciate! For the moment, I am using finite differences.

[jcarpent]

Solved by #1220.