Modifications to cube solving task:

- Switched from order 0 to order 1 splines. This makes the hand move more and avoids local minima.
- Wait mode now adds joint velocity cost to prevent jittering.
- Made the task harder: use default damping and armature, only frictionloss is reduced.
- Add printouts from the cube task transitions.

PiperOrigin-RevId: 602736932
Change-Id: I2bf3cbe9277c775361b8541415e53054b07b3c7d

Author: erez-tom

mjpc/tasks/cube/cube_3x3x3.xml.patch

@@ -23,7 +23,7 @@ diff --git a/cube_3x3x3_modified.xml b/cube_3x3x3_modified.xml
      <default class="cubelet">
 -      <joint type="ball" armature="0.0001" damping="0.0005" frictionloss="0.001"/>
 -      <geom type="mesh" condim="1" mesh="cubelet" euler="0 0 90"/>
-+      <joint type="ball" armature="0.00005" damping="0.0001" frictionloss="0.00005"/>
++      <joint type="ball" armature="0.0001" damping="0.0005" frictionloss="0.00005"/>
 +      <geom type="mesh" condim="1" mesh="cubelet" quat="1 0 0 1"/>
      </default>
      <default class="core">

mjpc/tasks/cube/solve.cc

@@ -15,6 +15,7 @@
 #include "mjpc/tasks/cube/solve.h"
 
 #include <algorithm>
+#include <iostream>
 #include <random>
 #include <string>
 
@@ -131,6 +132,7 @@ void CubeSolve::ResidualFn::Residual(const mjModel* model, const mjData* data,
 void CubeSolve::TransitionLocked(mjModel* model, mjData* data) {
   if (transition_model_) {
     if (mode == kModeWait) {
+      weight[11] = .01;  // add penalty on joint movement
       // wait
     } else if (mode == kModeScramble) {  // scramble
       double scramble_param = parameters[6];
@@ -190,9 +192,11 @@ void CubeSolve::TransitionLocked(mjModel* model, mjData* data) {
 
       // set face goal index
       goal_index_ = num_scramble - 1;
+      std::cout << "rotations required: " << num_scramble << "\n";
 
       // set to solve
       mode = kModeSolve;
+      weight[11] = 0;  // remove penalty on joint movement
     } else if (mode == kModeSolve) {  // solve
       // set goal
       mju_copy(parameters.data(), goal_cache_.data() + 6 * goal_index_, 6);
@@ -204,7 +208,9 @@ void CubeSolve::TransitionLocked(mjModel* model, mjData* data) {
       if (mju_norm(error, 6) < 0.085) {
         if (goal_index_ == 0) {
           mode = kModeWait;
+          std::cout << "solved!\n";
         } else {
+          std::cout << "rotations remaining: " << goal_index_ << "\n";
           goal_index_--;
         }
       }
@@ -213,11 +219,10 @@ void CubeSolve::TransitionLocked(mjModel* model, mjData* data) {
 
   // check for drop
   if (data->qpos[6] < kResetHeight) {
-    // reset cube position + orientation
-    mju_copy(data->qpos, model->key_qpos, 7);
+    if (mode != kModeWait) { std::cout << "cube fell\n"; }
 
-    // reset cube velocity
-    mju_zero(data->qvel, 6);
+    // stop optimization
+    mode = kModeWait;
   }
 
   // check goal index

mjpc/tasks/cube/task.xml

@@ -12,8 +12,8 @@
     <numeric name="agent_policy_width" data="0.0035" />
     <numeric name="sampling_spline_points" data="6" />
     <numeric name="sampling_exploration" data="0.1" />
-    <numeric name="sampling_trajectories" data="20" />
-    <numeric name="sampling_representation" data="0" />
+    <numeric name="sampling_trajectories" data="60" />
+    <numeric name="sampling_representation" data="1" />
     <!-- manual face goals -->
     <numeric name="residual_Red" data="0 -3.14 3.14"/>
     <numeric name="residual_Orange" data="0 -3.14 3.14"/>