Fix small bug showing up only in DEBUG mode for an inconsistency in size of...

Fix small bug showing up only in DEBUG mode for an inconsistency in size of the Kp, Kd vectors in ContactPoint

src/contacts/contact-point.cpp

@@ -117,10 +117,10 @@ void ContactPoint:: updateForceGeneratorMatrix()
 unsigned int ContactPoint::n_motion() const { return m_motionTask.dim(); }
 unsigned int ContactPoint::n_force() const { return 3; }
 
-const Vector & ContactPoint::Kp() const { return m_motionTask.Kp(); }
-const Vector & ContactPoint::Kd() const { return m_motionTask.Kd(); }
-void ContactPoint::Kp(ConstRefVector Kp){ m_motionTask.Kp(Kp); }
-void ContactPoint::Kd(ConstRefVector Kd){ m_motionTask.Kd(Kd); }
+const Vector & ContactPoint::Kp() const { return m_motionTask.Kp().head<3>(); }
+const Vector & ContactPoint::Kd() const { return m_motionTask.Kd().head<3>(); }
+void ContactPoint::Kp(ConstRefVector Kp){ Vector6 Kp6; Kp6.head<3>() = Kp; m_motionTask.Kp(Kp6); }
+void ContactPoint::Kd(ConstRefVector Kd){ Vector6 Kd6; Kd6.head<3>() = Kd; m_motionTask.Kd(Kd6); }
 
 bool ContactPoint::setContactNormal(ConstRefVector contactNormal)
 {