<commit_before>/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package org.etoile.animation.ode.controller;

import org.etoile.animation.ode.OdeHumanoid;
import org.etoile.animation.ode.OdeJoint;
import org.etoile.animation.ode.OdeRigidBody;
import org.etoile.animation.ode.QuaternionD;
import org.ode4j.math.DVector3;
import org.ode4j.math.DVector3C;

/**
 *
 * @author <NAME>
 */
public class BalanceController implements BaseController {

    String _name = "BalanceController";
    int _index;
    boolean _active = true;
    double[] _com = new double[]{0, 0, 0};
    double[] _comdiff = new double[]{0, 0, 0};
    double[] _offset = new double[]{0.00, 0, 0};

    private double kpx = 280f;
    private double kvx = 28f;

    private double kpz = 280f;
    private double kvz = 28f;

    private double khx = 150f;
    private double khz = 150f;

    private double kpKnee = 150f;
    private double kvKnee = 15f;

    private double rotXLFoot = 0;
    private double rotXRFoot = 0;

    private double rotZLFoot = 0;
    private double rotZRFoot = 0;

    private double lKneeC = 0;
    private double rKneeC = 0;

    private double lHipC = 0;
    private double rHipC = 0;

    private double lHipCZ = 0;
    private double rHipCZ = 0;

    private double ul, ll, pelH = 0.795f;

    private double s = 1;

    OdeHumanoid _human;
    OdeRigidBody leftFoot;
    OdeRigidBody rightFoot;

    OdeJoint leftAnkle;
    OdeJoint rightAnkle;
    OdeJoint rightHip;
    OdeJoint leftHip;
    OdeJoint rightKnee;
    OdeJoint leftKnee;

    public BalanceController(OdeHumanoid human) {
        _human = human;
        _human.addController(this);
        leftFoot = _human.getBody("foot_l");
        rightFoot = _human.getBody("foot_r");

        leftAnkle = _human.getJoint("l_ankle");
        rightAnkle = _human.getJoint("r_ankle");
        rightHip = _human.getJoint("r_hip");
        leftHip = _human.getJoint("l_hip");
        rightKnee = _human.getJoint("r_knee");
        leftKnee = _human.getJoint("l_knee");
        
        if (leftHip != null && leftKnee != null && leftAnkle != null)
        {
            double hip[] = leftHip.getJointAnchor(0);
            double knee[] = leftKnee.getJointAnchor(0);
            double ankle[] = leftAnkle.getJointAnchor(0);
         
            DVector3 hipP = new DVector3(hip[0], hip[1], hip[2]);
            DVector3 kneeP = new DVector3(knee[0], knee[1], knee[2]);
            DVector3 ankleP = new DVector3(ankle[0], ankle[1], ankle[2]);
            ul = hipP.sub(kneeP).length();
            ll = kneeP.sub(ankleP).length();
            //logger.debug("setPhysicalHumanoid {} ul={} ll={}", new Object[]{p.getId(),ul, ll});            
        }
        _human.updateCOM(0);
    }

    @Override
    public void update(double dt) {

        _human.updateCOM(dt);
        _human.getCOM(_com);
        _human.getCOMDiff(_comdiff);
        //System.out.println(_com[0] + " " + _com[1] + " " + _com[2]);
        DVector3C leftFootPos = leftFoot.getPosition();
        DVector3C rightFootPos = rightFoot.getPosition();
        
        QuaternionD qLeftFoot = leftFoot.getRotation();
        QuaternionD qRightFoot = rightFoot.getRotation();
        QuaternionD qBody = QuaternionD.slerp(qLeftFoot, qRightFoot, 0.5, true);
        qBody.invert();

        DVector3 com = new DVector3(_com[0], _com[1], _com[2]);
        DVector3 comdiff = new DVector3(_comdiff[0], _comdiff[1], _comdiff[2]);
        com = qBody.rotate(com);
        comdiff = qBody.rotate(comdiff);
        _com[0] = com.get0();
        _com[1] = com.get1();
        _com[2] = com.get2();
        _comdiff[0] = comdiff.get0();
        _comdiff[1] = comdiff.get1();
        _comdiff[2] = comdiff.get2();
        double[] centerfeet = new double[]{0, 0, 0};
        centerfeet[0] = (leftFootPos.get0() + rightFootPos.get0()) * 0.5 + _offset[0];
        centerfeet[1] = (leftFootPos.get1() + rightFootPos.get1()) * 0.5 + _offset[1];
        centerfeet[2] = (leftFootPos.get2() + rightFootPos.get2()) * 0.5 + _offset[2];
        
        //System.out.println("COM: " + _com[0] + " " +_com[1] +" "+_com[2] + "DIF: "+_comdiff[0] + " " +_comdiff[1] +" "+_comdiff[2]);
        //System.out.println("feet C: " + centerfeet[0] + " " +centerfeet[1] +" "+centerfeet[2]);
        
        double offsetX = s * kpx * (centerfeet[0] - _com[0]) - kvx * _comdiff[0];
        double offsetZ = s * kpz * (centerfeet[2] - _com[2]) - kvz * _comdiff[2];
        
        // keep balance
        //z y x torque   offsetX apply on Z axis, offsetZ apply on x axis
        leftHip.addTorque(-offsetZ, 0f, -offsetX);
        rightHip.addTorque(-offsetZ, 0f, -offsetX);

        double hipD;
        // System.out.println("pelvis height "+pelH+" "+(ul+ll));
        if (pelH >= ul + ll) {
            hipD = 0;
        } else {
            hipD = -(float) Math.acos((ul * ul - ll * ll + pelH * pelH) / (2 * ul * pelH));
        }
        //System.out.println("hipD " + hipD);

        //x is the third axis
        double dHipR = 0;
        double dHipL = 0;
        double rHipOld = rHipC;
        double lHipOld = lHipC;
        lHipC = leftHip.getAngle(2);
        rHipC = rightHip.getAngle(2);
        dHipR = (rHipC - rHipOld) / dt;
        dHipL = (lHipC - lHipOld) / dt;
        //System.out.println("lHipC " + lHipC);
        double torqueL = ((hipD - lHipC) * s * khx - dHipL * s * 15f);
        double torqueR = ((hipD - rHipC) * s * khx - dHipR * s * 15f);
        //leftHip.addTorque(torqueL, 0, 0);
        //rightHip.addTorque(torqueR, 0, 0);

        //z is the 1st axis
        float hipDZ = 0;
        double dHipRZ = 0;
        double dHipLZ = 0;
        double rHipOldZ = rHipCZ;
        double lHipOldZ = lHipCZ;
        lHipCZ = leftHip.getAngle(0);
        rHipCZ = rightHip.getAngle(0);
        dHipRZ = (rHipCZ - rHipOldZ) / dt;
        dHipLZ = (lHipCZ - lHipOldZ) / dt;

        torqueL = (hipDZ - lHipCZ) * s * khz - dHipLZ * s * 15f;
        torqueR = (hipDZ - rHipCZ) * s * khz - dHipRZ * s * 15f;
        leftHip.addTorque(0, 0, torqueL);
        rightHip.addTorque(0, 0, torqueR);

        //x is the 3rd axis
        double kneeD;
        if (pelH >= ul + ll) {
            kneeD = 0;
        } else {
            kneeD = (Math.PI - Math.acos((ul * ul + ll * ll - pelH * pelH) / (2 * ul * ll)));
        }
        //System.out.println("kneeD " + kneeD);
        
        double dKneeR = 0;
        double dKneeL = 0;
        double rKneeOld = rKneeC;
        double lKneeOld = lKneeC;
        lKneeC = leftKnee.getAngle(0);
        rKneeC = rightKnee.getAngle(0);
        dKneeR = (rKneeC - rKneeOld) / dt;
        dKneeL = (lKneeC - lKneeOld) / dt;
        //System.out.println("lKneeC " + lKneeC);
        double lKneeT = (kneeD - lKneeC);
        double rKneeT = (kneeD - rKneeC);
<commit_msg>one version seems to work<commit_after>/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package org.etoile.animation.ode.controller;

import org.etoile.animation.ode.OdeHumanoid;
import org.etoile.animation.ode.OdeJoint;
import org.etoile.animation.ode.OdeRigidBody;
import org.etoile.animation.ode.QuaternionD;
import org.ode4j.math.DVector3;
import org.ode4j.math.DVector3C;

/**
 *
 * @author <NAME>
 */
public class BalanceController implements BaseController {

    String _name = "BalanceController";
    int _index;
    boolean _active = true;
    double[] _com = new double[]{0, 0, 0};
    double[] _comdiff = new double[]{0, 0, 0};
    double[] _offset = new double[]{0.00, 0, 0};

    private double kpx = 280f;
    private double kvx = 28f;

    private double kpz = 280f;
    private double kvz = 28f;

    private double khx = 150f;
    private double khz = 150f;

    private double kpKnee = 150f;
    private double kvKnee = 15f;

    private double rotXLFoot = 0;
    private double rotXRFoot = 0;

    private double rotZLFoot = 0;
    private double rotZRFoot = 0;

    private double lKneeC = 0;
    private double rKneeC = 0;

    private double lHipC = 0;
    private double rHipC = 0;

    private double lHipCZ = 0;
    private double rHipCZ = 0;

    private double ul, ll, pelH = 0.795f;

    private double s = 1;

    OdeHumanoid _human;
    OdeRigidBody leftFoot;
    OdeRigidBody rightFoot;

    OdeJoint leftAnkle;
    OdeJoint rightAnkle;
    OdeJoint rightHip;
    OdeJoint leftHip;
    OdeJoint rightKnee;
    OdeJoint leftKnee;

    public BalanceController(OdeHumanoid human) {
        _human = human;
        _human.addController(this);
        leftFoot = _human.getBody("foot_l");
        rightFoot = _human.getBody("foot_r");

        leftAnkle = _human.getJoint("l_ankle");
        rightAnkle = _human.getJoint("r_ankle");
        rightHip = _human.getJoint("r_hip");
        leftHip = _human.getJoint("l_hip");
        rightKnee = _human.getJoint("r_knee");
        leftKnee = _human.getJoint("l_knee");
        
        if (leftHip != null && leftKnee != null && leftAnkle != null)
        {
            double hip[] = leftHip.getJointAnchor(0);
            double knee[] = leftKnee.getJointAnchor(0);
            double ankle[] = leftAnkle.getJointAnchor(0);
         
            DVector3 hipP = new DVector3(hip[0], hip[1], hip[2]);
            DVector3 kneeP = new DVector3(knee[0], knee[1], knee[2]);
            DVector3 ankleP = new DVector3(ankle[0], ankle[1], ankle[2]);
            ul = hipP.sub(kneeP).length();
            ll = kneeP.sub(ankleP).length();
            //logger.debug("setPhysicalHumanoid {} ul={} ll={}", new Object[]{p.getId(),ul, ll});            
        }
        _human.updateCOM(0);
    }

    @Override
    public void update(double dt) {

        _human.updateCOM(dt);
        _human.getCOM(_com);
        _human.getCOMDiff(_comdiff);
        //System.out.println(_com[0] + " " + _com[1] + " " + _com[2]);
        DVector3C leftFootPos = leftFoot.getPosition();
        DVector3C rightFootPos = rightFoot.getPosition();
        
        QuaternionD qLeftFoot = leftFoot.getRotation();
        QuaternionD qRightFoot = rightFoot.getRotation();
        QuaternionD qBody = QuaternionD.slerp(qLeftFoot, qRightFoot, 0.5, true);
        qBody.invert();

        DVector3 com = new DVector3(_com[0], _com[1], _com[2]);
        DVector3 comdiff = new DVector3(_comdiff[0], _comdiff[1], _comdiff[2]);
        com = qBody.rotate(com);
        comdiff = qBody.rotate(comdiff);
        _com[0] = com.get0();
        _com[1] = com.get1();
        _com[2] = com.get2();
        _comdiff[0] = comdiff.get0();
        _comdiff[1] = comdiff.get1();
        _comdiff[2] = comdiff.get2();
        double[] centerfeet = new double[]{0, 0, 0};
        centerfeet[0] = (leftFootPos.get0() + rightFootPos.get0()) * 0.5 + _offset[0];
        centerfeet[1] = (leftFootPos.get1() + rightFootPos.get1()) * 0.5 + _offset[1];
        centerfeet[2] = (leftFootPos.get2() + rightFootPos.get2()) * 0.5 + _offset[2];
        
        //System.out.println("COM: " + _com[0] + " " +_com[1] +" "+_com[2] + "DIF: "+_comdiff[0] + " " +_comdiff[1] +" "+_comdiff[2]);
        //System.out.println("feet C: " + centerfeet[0] + " " +centerfeet[1] +" "+centerfeet[2]);
        
        double offsetX = s * kpx * (centerfeet[0] - _com[0]) - kvx * _comdiff[0];
        double offsetZ = s * kpz * (centerfeet[2] - _com[2]) - kvz * _comdiff[2];
        
        // keep balance
        //z y x torque   offsetX apply on Z axis, offsetZ apply on x axis
        leftHip.addTorque(-offsetZ, 0f, -offsetX);
        rightHip.addTorque(-offsetZ, 0f, -offsetX);

        double hipD;
        // System.out.println("pelvis height "+pelH+" "+(ul+ll));
        if (pelH >= ul + ll) {
            hipD = 0;
        } else {
            hipD = (float) Math.acos((ul * ul - ll * ll + pelH * pelH) / (2 * ul * pelH));
        }
        //System.out.println("hipD " + hipD);

        //x is the third axis
        double dHipR = 0;
        double dHipL = 0;
        double rHipOld = rHipC;
        double lHipOld = lHipC;
        lHipC = -leftHip.getAngle(0);
        rHipC = -rightHip.getAngle(0);
        dHipR = (rHipC - rHipOld) / dt;
        dHipL = (lHipC - lHipOld) / dt;
        //System.out.println("lHipC " + lHipC);
        double torqueL = ((hipD - lHipC) * s * khx - dHipL * s * 15f);
        double torqueR = ((hipD - rHipC) * s * khx - dHipR * s * 15f);
        leftHip.addTorque(-torqueL, 0, 0);
        rightHip.addTorque(-torqueR, 0, 0);

        //z is the 1st axis
        float hipDZ = 0;
        double dHipRZ = 0;
        double dHipLZ = 0;
        double rHipOldZ = rHipCZ;
        double lHipOldZ = lHipCZ;
        lHipCZ = leftHip.getAngle(2);
        rHipCZ = rightHip.getAngle(2);
        dHipRZ = (rHipCZ - rHipOldZ) / dt;
        dHipLZ = (lHipCZ - lHipOldZ) / dt;

        torqueL = (hipDZ - lHipCZ) * s * khz - dHipLZ * s * 15f;
        torqueR = (hipDZ - rHipCZ) * s * khz - dHipRZ * s * 15f;
        leftHip.addTorque(0, 0, torqueL);
        rightHip.addTorque(0, 0, torqueR);

        //x is the 3rd axis
        double kneeD;
        if (pelH >= ul + ll) {
            kneeD = 0;
        } else {
            kneeD = (Math.PI - Math.acos((ul * ul + ll * ll - pelH * pelH) / (2 * ul * ll)));
        }
        //System.out.println("kneeD " + kneeD);
        
        double dKneeR = 0;
        double dKneeL = 0;
        double rKneeOld = rKneeC;
        double lKneeOld = lKneeC;
        lKneeC = leftKnee.getAngle(0);
        rKneeC = rightKnee.getAngle(0);
        dKneeR = (rKneeC - rKneeOld) / dt;
        dKneeL = (lKneeC - lKneeOld) / dt;
        //System.out.println("lKneeC " + lKneeC);
        double lKneeT = (kneeD - lKneeC);
        double rKneeT = (kneeD - rKneeC);

{'fraction_non_alphanumeric': 0.1383842010771993, 'fraction_numerical': 0.024703770197486534, 'mean_word_length': 2.1838134430727023, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 9, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3523684', 'n_tokens_mistral': 5462, 'n_tokens_neox': 5106, 'n_words': 1138}