fix a problem with free3e-exp and spherical-exp joint

core/projects/redmax/Joint/JointFree3DExp.cpp

@@ -103,7 +103,9 @@ void JointFree3DExp::update(bool design_gradient) {
 }
 
 bool JointFree3DExp::reparam() {
-    return _joint_spherical_exp->reparam();
+    bool flag = _joint_spherical_exp->reparam();
+    _q.tail(3) = _joint_spherical_exp->_q;
+    _qdot.tail(3) = _joint_spherical_exp->_qdot;
+    return flag;
 }
-
 }

core/projects/redmax/Joint/JointSphericalExp.cpp

@@ -237,22 +237,20 @@ void JointSphericalExp::inner_update() {
 bool JointSphericalExp::reparam() {
     // if q.norm is close to 2pi, then reparameterize as q = (1 - 2pi / q.norm) * q
     dtype qnorm = _q.norm();
-    if (qnorm > (dtype)1.5 * constants::pi) {
-        while (qnorm > (dtype)1.5 * constants::pi) {
+    if (qnorm > (dtype)1.1 * constants::pi) {
+        Matrix3 S0 = _S_j.topLeftCorner(3, 3);
+
+        while (qnorm > (dtype)1.1 * constants::pi) {
             _q = ((dtype)1.0 - 2.0 * constants::pi / qnorm) * _q;
             qnorm = _q.norm();
+            std::cerr << "reparam" << std::endl;
         }
-        Matrix3 S0 = _S_j.topLeftCorner(3, 3);
-        std::cerr << "phi0 = " << (S0 * _qdot).transpose() << std::endl;
-        // std::cerr << "S = " << _S_j << std::endl;
-        std::cerr << "Q0 = " << _Q << std::endl;
+
         inner_update();
+
         Matrix3 S1 = _S_j.topLeftCorner(3, 3);
-        _qdot = S1.inverse() * S0 * _qdot;
-        std::cerr << "phi1 = " << (S1 * _qdot).transpose() << std::endl;
-        // std::cerr << "S = " << _S_j << std::endl;
-        std::cerr << "Q1 = " << _Q << std::endl;
-        inner_update();
+        _qdot = S1.inverse() * S0 * _qdot; // just need to compute new q and new qdot, the remaining update will be performed by a global update() function
+
         return true;
     } else {
         return false;

core/projects/redmax/Robot.cpp

@@ -160,11 +160,14 @@ bool Robot::check_valid() {
 }
 
 void Robot::reparam() {
+    bool reparam_flag = false;
     for (auto joint : _joints) {
-        joint->reparam();
+        reparam_flag |= joint->reparam();
     }
 
-    update();
+    if (reparam_flag) {
+        update();
+    }
 }
 
 void Robot::set_q(const VectorX q) {
@@ -603,7 +606,7 @@ void Robot::computeJointJacobianWithDerivative(MatrixX& J, MatrixX& Jdot, Jacobi
     // compute joint jacobian J_mr: phi = J_mr * qdot
     // compute its time derivative Jdot
     // compute their derivative w.r.t q
-    // follow pseudo-code Slgorithm (11)
+    // follow pseudo-code Algorithm (11)
     J = MatrixX::Zero(_ndof_m, _ndof_r);
     Jdot = MatrixX::Zero(_ndof_m, _ndof_r);
     dJ_dq = JacobianMatrixVector(_ndof_m, _ndof_r, _ndof_r);

core/projects/redmax/Simulation.cpp

@@ -707,7 +707,7 @@ void Simulation::forward(int num_steps, bool verbose, bool test_derivatives) {
         }
 
         set_state(q_next, qdot_next);
-        reparam();
+        // reparam();
         q_next = get_q();
         qdot_next = get_qdot();
 

examples/test_redmax.py

@@ -6,11 +6,13 @@
 import redmax_py as redmax
 import os
 import argparse
+import time
 
 if __name__ == '__main__':
     parser = argparse.ArgumentParser('Test forward and rendering of simulation')
     parser.add_argument("--model", type = str, default = 'finger_torque') 
-
+    parser.add_argument('--gradient', action = 'store_true')
+
     args = parser.parse_args()
 
     asset_folder = os.path.abspath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../assets'))
@@ -24,17 +26,43 @@
 
     num_steps = 1000
 
-    sim.reset(backward_flag = False) # reset the simulation to start a new trajectory
+    sim.reset(backward_flag = args.gradient) # reset the simulation to start a new trajectory
 
     ndof_u = sim.ndof_u # number of actions
     ndof_r = sim.ndof_r # number of degrees of freedom
     ndof_var = sim.ndof_var # number of auxiliary variables
+    loss = 0.
+    df_du = np.zeros(ndof_u * num_steps)
+    df_dq = np.zeros(ndof_r * num_steps)
 
+    t0 = time.time()
     for i in range(num_steps):
-        sim.set_u(np.zeros(ndof_u))
+        # sim.set_u(np.zeros(ndof_u))
+        sim.set_u((np.random.rand(ndof_u) - 0.5) * 2.)
+        # sim.set_u(np.ones(ndof_u))
+        # sim.set_u(np.ones(ndof_u) * np.sin(i / 100 * np.pi))
+        # if i < 50:
+        #     sim.set_u(np.ones(ndof_u) * -1)
+        # else:
+        #     sim.set_u(np.ones(ndof_u))
         sim.forward(1, verbose = False)
-        q = sim.get_q() # get state of the simulation, |q| = ndof_r
-        qdot = sim.get_qdot() # get the velocity of the state, |qdot| = ndof_r
-        variables = sim.get_variables() # get the auxiliary varibles
-
+        q = sim.get_q()
+        loss += np.sum(q)
+        df_dq[ndof_u * i:ndof_u * (i + 1)] = 1.
+
+    t1 = time.time()
+
+    if args.gradient:
+        sim.backward_info.set_flags(False, False, False, True)
+        sim.backward_info.df_du = df_du
+        sim.backward_info.df_dq = df_dq
+        sim.backward()
+
+    t2 = time.time()
+
+    fps_forward_only = num_steps / (t1 - t0)
+    fps_with_gradient = num_steps / (t2 - t0)
+
+    print('FPS (forward only) = {:.1f}, FPS (with gradient) = {:.1f}'.format(fps_forward_only, fps_with_gradient))
+
     SimRenderer.replay(sim, record = False) # render the simulation replay video