Possible bug at the code and a question

[Luis Filipe Rossi]

Hi, 

I am trying to understand the cartwheel controller and I think I found a bug at the code:

At the IKVMCController.cpp file at the function COMJT we have 

m = tibia->props.mass + femur->props.mass + pelvis->props.mass + lBack->props.mass + mBack->props.mass;

Vector3d fA = computeVirtualForce();

Vector3d f1 = Vector3d(anklePos, tibia->state.position) * tibia->props.mass +

Vector3d(anklePos, femur->state.position) * femur->props.mass + 

Vector3d(anklePos, pelvis->state.position) * pelvis->props.mass + 

Vector3d(anklePos, lBack->state.position) * lBack->props.mass + 

Vector3d(anklePos, mBack->state.position) * mBack->props.mass;

f1 /= m;

Vector3d f2 = Vector3d(kneePos, femur->state.position) * femur->props.mass + 

Vector3d(kneePos, pelvis->state.position) * pelvis->props.mass + 

Vector3d(kneePos, lBack->state.position) * lBack->props.mass + 

Vector3d(kneePos, mBack->state.position) * mBack->props.mass;

f2 /= m;

Vector3d f3 = Vector3d(hipPos, pelvis->state.position) * pelvis->props.mass + 

Vector3d(hipPos, lBack->state.position) * lBack->props.mass + 

Vector3d(hipPos, mBack->state.position) * mBack->props.mass;

f3 /= m;

Vector3d f4 = Vector3d(lbackPos, lBack->state.position) * lBack->props.mass + 

Vector3d(lbackPos, mBack->state.position) * mBack->props.mass;

f4 /= m;

Vector3d f5 = Vector3d(mbackPos, mBack->state.position) * mBack->props.mass;

f5 /= m;

Does dividing all the fx by the total mass makes sense? In my understanding the fx is the COM that the joints sees so it should be the sum of the P x M of every link that this joint sustain divided by the sum of the masses of these links and not all the links. 

Second, I have a question regarding the implementation. If we are using the Jacobian to generate a virtual Force to Accelerate the the COM at the horizontal component, what is sustaining the COM? Basically there is no guarantee that the COM will remain at some defined height as it can still fall while accelerating as desired. Shouldnt be a Virtual Force to sustain the COM? Maybe a Spring Dumper or a Zero Gravity?

Thanl you!