ghost nodes for positional and jointmove

osudrl · Aug 10, 2018 · 4ca0e60 · 4ca0e60
1 parent b2e5da4
commit 4ca0e60
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 3 deletions.
diff --git a/src/decor.c b/src/decor.c
@@ -36,7 +36,7 @@ bool decor_has_init(traj_info_t* traj_info)
     return DEC.count >= 0;
 }
 
-void decor_positional_rootframe(traj_info_t* traj_info, double* pos)
+void decor_positional_init(traj_info_t* traj_info, double* pos)
 {
     DEC.count = 0;
     DEC.pos[DEC.count * 3 + 0] = pos[0];
@@ -59,6 +59,29 @@ void decor_positional_addto(traj_info_t* traj_info, double* pos)
     DEC.count++;
 }
 
+void decor_joint_init(traj_info_t* traj_info, double* pos)
+{
+    DEC.count = 0;
+    DEC.pos[DEC.count * 3 + 0] = pos[0];
+    DEC.pos[DEC.count * 3 + 1] = pos[1];
+    DEC.pos[DEC.count * 3 + 2] = pos[2];
+    mju_copy(DEC.size + DEC.count * 3, DEC.size_default_joint, 3);
+    f_copy(DEC.rgba + DEC.count * 4, DEC.rgba_default_joint, 4);
+    DEC.rgba[DEC.count * 4 + 3] = 0.25;
+    DEC.count++;
+}
+
+void decor_joint_addto(traj_info_t* traj_info, double* pos)
+{
+    DEC.pos[DEC.count * 3 + 0] = pos[0];
+    DEC.pos[DEC.count * 3 + 1] = pos[1];
+    DEC.pos[DEC.count * 3 + 2] = pos[2];
+    mju_copy(DEC.size + DEC.count * 3, DEC.size_default_joint, 3);
+    f_copy(DEC.rgba + DEC.count * 4, DEC.rgba_default_joint, 4);
+    DEC.rgba[DEC.count * 4 + 3] = 0.25;
+    DEC.count++;
+}
+
 
 
 

diff --git a/src/decor.h b/src/decor.h
@@ -6,10 +6,15 @@
 #include "mujoco.h"
 #include <stdbool.h>
 
+#include "decor.h"
+
+void decor_single_sphere(traj_info_t* traj_info, mjvScene* scn, int num);
 void decor_showdecor(traj_info_t* traj_info, mjvScene* scn);
 void decor_reset(traj_info_t* traj_info);
 bool decor_has_init(traj_info_t* traj_info);
-void decor_positional_rootframe(traj_info_t* traj_info, double* pos);
+void decor_positional_init(traj_info_t* traj_info, double* pos);
 void decor_positional_addto(traj_info_t* traj_info, double* pos);
+void decor_joint_init(traj_info_t* traj_info, double* pos);
+void decor_joint_addto(traj_info_t* traj_info, double* pos);
 
 #endif
diff --git a/src/node.c b/src/node.c
@@ -511,13 +511,16 @@ void node_position_jointmove(traj_info_t* traj_info,
     double rootframe_init;
     double filter;
     double temp_new_qpos_val;
+    bool update_decor;
 
     timeline_set_qposes_to_pose_frame(
         traj_info,
         traj_info->timeline,
         rootframe);
     rootframe_init = traj_info->d->qpos[SEL.jointnum];
 
+    update_decor = !decor_has_init(traj_info);
+
     for (i = 0; i < NODECOUNT; i++)
     {
         frame = (traj_info->timeline->numposes / NODECOUNT) * i;
@@ -552,6 +555,11 @@ void node_position_jointmove(traj_info_t* traj_info,
             traj_info->d->xquat + (4*body_id.id),
             body_id.id
             );
+        if(update_decor && i == 0)
+            decor_joint_init(traj_info, node_qpos);
+        else if (update_decor)
+            decor_joint_addto(traj_info, node_qpos);
+
     }
 }
 
@@ -636,7 +644,7 @@ void node_scale_visually_positional(
         decor_update = 0;
 
     if(decor_update)
-        decor_positional_rootframe(traj_info, global_body_init_xpos_at_rootframe);
+        decor_positional_init(traj_info, global_body_init_xpos_at_rootframe);
 
     for (i = 0; i < NODECOUNT; i++)
     {