From 393928a468e8ecadd58b36d52fa0d246503e4cd1 Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Tue, 28 Aug 2018 15:55:31 -0700 Subject: [PATCH 01/19] timeline truncate and timeline loopfuncs very successfull --- src/node.c | 16 +++++++----- src/timeline.c | 66 ++++++++++++++++++++++++++++++++++++++++++++++++-- src/timeline.h | 2 ++ 3 files changed, 76 insertions(+), 8 deletions(-) diff --git a/src/node.c b/src/node.c index c467a47..d07c5ca 100644 --- a/src/node.c +++ b/src/node.c @@ -318,17 +318,18 @@ void node_perform_pert( int loopcount; timeline_t* timeline_old; timeline_t* timeline_new; + timeline_t* timeline_final; bool failed; failed = 0; timeline_old = traj_info->timeline; - timeline_new = timeline_duplicate(timeline_old); - rootframe = timeline_make_frame_safe(rootframe, timeline_old->numposes); + timeline_new = timeline_truncate(timeline_old, timeline_old->numposes/2); + rootframe = timeline_make_frame_safe(rootframe, timeline_new->numposes); node_calc_frame_lowhigh( &low_frame, &high_frame, rootframe, - timeline_old->numposes, + timeline_new->numposes, traj_info); init_time = traj_calculate_runtime_micros(traj_info); @@ -338,7 +339,7 @@ void node_perform_pert( node_calclate_global_target_using_transformation_type( traj_info, - timeline_old, + timeline_new, global_body_init_xpos_at_rootframe, global_body_target_xpos, rootframe_transform_vector, @@ -387,7 +388,7 @@ void node_perform_pert( { node_calclate_global_target_using_transformation_type( traj_info, - timeline_old, + timeline_new, global_body_init_xpos_at_rootframe, global_body_target_xpos, rootframe_transform_vector, @@ -416,7 +417,7 @@ void node_perform_pert( node_calclate_global_target_using_transformation_type( traj_info, - timeline_old, + timeline_new, global_body_init_xpos_at_rootframe, global_body_target_xpos, rootframe_transform_vector, @@ -460,6 +461,9 @@ void node_perform_pert( (iktimedelta/1000000.0), 1000.0*params->ik_accuracy_cutoff); + timeline_final = timeline_loop(timeline_new,2); + timeline_free(timeline_new); + timeline_new = timeline_final; timeline_safe_link(timeline_new, timeline_old); traj_info->timeline = timeline_new; timeline_new->node_type = NODE_POSITIONAL; diff --git a/src/timeline.c b/src/timeline.c index 391000b..fa672b9 100644 --- a/src/timeline.c +++ b/src/timeline.c @@ -48,7 +48,7 @@ void timeiline_init_from_input_file(traj_info_t* traj_info) int start; int loopcount; - loopcount = 1; + loopcount = 2; bytecount = timeline_fill_full_traj_state_array(traj_info, (uint8_t**) &fulls); bytecount /= sizeof(full_traj_state_t); @@ -158,6 +158,69 @@ timeline_t* timeline_init_with_single_pose(qpos_t* qpos, timeline_t* xcopy) return dest; } + + +timeline_t* timeline_loop(timeline_t* ref, int loopcount) +{ + timeline_t* dest; + int qposbytecount; + int i; + int big; + int start; + + qposbytecount = sizeof(qpos_t) * ref->numposes * loopcount; + + dest = malloc(sizeof(timeline_t)); + dest->qposes = malloc(qposbytecount); + + for(i = 0; i < ref->numposes * loopcount; i++) + { + mju_copy(dest->qposes[i].q, + ref->qposes[i % ref->numposes].q, + CASSIE_QPOS_SIZE); + } + + for(big = 1; big <= loopcount-1; big++) + { + start = ref->numposes * big; + for(i = start; i < start + ref->numposes; i++) + { + mju_add(dest->qposes[i].q, + dest->qposes[i].q, + dest->qposes[start-1].q, + 1); + } + } + + dest->next = NULL; + dest->prev = NULL; + dest->numposes = ref->numposes * loopcount; + dest->duration = ref->duration; + dest->node_type = NODE_NONE; + + return dest; +} + +timeline_t* timeline_truncate(timeline_t* ref, int numposes) +{ + timeline_t* dest; + int qposbytecount; + + qposbytecount = sizeof(qpos_t) * numposes; + + dest = malloc(sizeof(timeline_t)); + dest->qposes = malloc(qposbytecount); + + memcpy(dest->qposes, ref->qposes, qposbytecount); + dest->next = NULL; + dest->prev = NULL; + dest->numposes = numposes; + dest->duration = ref->duration; + dest->node_type = NODE_NONE; + + return dest; +} + timeline_t* timeline_duplicate(timeline_t* ref) { timeline_t* dest; @@ -175,7 +238,6 @@ timeline_t* timeline_duplicate(timeline_t* ref) dest->duration = ref->duration; dest->node_type = NODE_NONE; - return dest; } diff --git a/src/timeline.h b/src/timeline.h index 45880a9..a050847 100644 --- a/src/timeline.h +++ b/src/timeline.h @@ -23,6 +23,8 @@ typedef struct _full_traj_state_t_ full_traj_state_t; timeline_t* timeline_init_with_single_pose(qpos_t* qpos, timeline_t* xcopy); +timeline_t* timeline_loop(timeline_t* ref, int loopcount); +timeline_t* timeline_truncate(timeline_t* ref, int numposes); timeline_t* timeline_duplicate(timeline_t* ref); void timeline_safe_link(timeline_t* next, timeline_t* prev); void timeline_free(timeline_t* ref); From 7d6c60db011151f9ff356c182bc6f8904916d76f Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Tue, 28 Aug 2018 16:20:34 -0700 Subject: [PATCH 02/19] kinda wacky fixes but working towards #29 --- src/node.c | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/node.c b/src/node.c index d07c5ca..3390388 100644 --- a/src/node.c +++ b/src/node.c @@ -140,11 +140,22 @@ void node_calclate_global_target_using_transformation_type( { double filter; v3_t body_init_xpos; + double plus,minus; filter = node_calculate_filter_from_frame_offset( frame_offset, SEL.nodesigma, SEL.nodeheight); + plus = node_calculate_filter_from_frame_offset( + frame_offset + traj_info->timeline->numposes/2, + SEL.nodesigma, + SEL.nodeheight); + minus = node_calculate_filter_from_frame_offset( + frame_offset - traj_info->timeline->numposes/2, + SEL.nodesigma, + SEL.nodeheight); + filter = mju_max(mju_max(plus,minus),filter); + body_init_xpos = node_get_body_xpos_by_frame( traj_info, timeline, From 7266c25fee1cf34e516598efc2c163d3196c32c1 Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Wed, 29 Aug 2018 15:34:26 -0700 Subject: [PATCH 03/19] rename numposes to numframes --- src/main.h | 3 ++- src/node.c | 39 +++++++++++++++++++----------------- src/overlay.c | 4 ++-- src/simulate.c | 2 +- src/timeline.c | 54 ++++++++++++++++++++++++-------------------------- 5 files changed, 52 insertions(+), 50 deletions(-) diff --git a/src/main.h b/src/main.h index 4fcd0d5..4ebbef0 100644 --- a/src/main.h +++ b/src/main.h @@ -40,7 +40,8 @@ enum scale_type_e struct _timeline_t_ { - int numposes; + int numframes; + int numnoloopframes; double duration; qpos_t* qposes; enum node_type_e node_type; diff --git a/src/node.c b/src/node.c index 3390388..edec797 100644 --- a/src/node.c +++ b/src/node.c @@ -146,12 +146,15 @@ void node_calclate_global_target_using_transformation_type( frame_offset, SEL.nodesigma, SEL.nodeheight); + + #warning "fix this" + plus = node_calculate_filter_from_frame_offset( - frame_offset + traj_info->timeline->numposes/2, + frame_offset + traj_info->timeline->numframes/2, SEL.nodesigma, SEL.nodeheight); minus = node_calculate_filter_from_frame_offset( - frame_offset - traj_info->timeline->numposes/2, + frame_offset - traj_info->timeline->numframes/2, SEL.nodesigma, SEL.nodeheight); filter = mju_max(mju_max(plus,minus),filter); @@ -180,11 +183,11 @@ int get_frame_from_node_body_id(traj_info_t* traj_info, int frame; offset = SEL.frame_offset; while(offset < 0) - offset += timeline->numposes; - offset = SEL.frame_offset % timeline->numposes; - frame = (timeline->numposes / NODECOUNT) * (node_id.id - 26); // or maybe 28 + offset += timeline->numframes; + offset = SEL.frame_offset % timeline->numframes; + frame = (timeline->numframes / NODECOUNT) * (node_id.id - 26); // or maybe 28 frame += offset; - frame %= timeline->numposes; + frame %= timeline->numframes; return frame; } @@ -240,7 +243,7 @@ void node_refine_pert( timeline_new = timeline_duplicate(timeline_old); init_time = traj_calculate_runtime_micros(traj_info); - rootframe = timeline_make_frame_safe(rootframe, timeline_old->numposes); + rootframe = timeline_make_frame_safe(rootframe, timeline_old->numframes); for(i = 0; i < traj_info->target_list_size; i++) { frame = rootframe + traj_info->target_list[i].frame_offset; @@ -280,7 +283,7 @@ void node_calc_frame_lowhigh( int* low_frame, int* high_frame, int rootframe, - int numposes, + int numframes, traj_info_t* traj_info) { int i; @@ -299,7 +302,7 @@ void node_calc_frame_lowhigh( if(!SEL.loop_enabled) { *low_frame = mju_max(0, rootframe - i); - *high_frame = mju_min(numposes-1, rootframe + i); + *high_frame = mju_min(numframes-1, rootframe + i); } else { @@ -334,13 +337,13 @@ void node_perform_pert( failed = 0; timeline_old = traj_info->timeline; - timeline_new = timeline_truncate(timeline_old, timeline_old->numposes/2); - rootframe = timeline_make_frame_safe(rootframe, timeline_new->numposes); + timeline_new = timeline_truncate(timeline_old, timeline_old->numframes/2); + rootframe = timeline_make_frame_safe(rootframe, timeline_new->numframes); node_calc_frame_lowhigh( &low_frame, &high_frame, rootframe, - timeline_new->numposes, + timeline_new->numframes, traj_info); init_time = traj_calculate_runtime_micros(traj_info); @@ -522,7 +525,7 @@ void node_dropped_jointmove( jointdiff = node_caluclate_jointdiff(traj_info, node_get_body_xpos_curr(traj_info, body_id)); - for (frame = 0; frame < timeline_new->numposes; frame++) + for (frame = 0; frame < timeline_new->numframes; frame++) { filter = node_calculate_filter_from_frame_offset( frame - rootframe, @@ -723,23 +726,23 @@ void node_compare_looped_filters( if(SEL.loop_enabled && filter < node_calculate_filter_from_frame_offset( - oldcurrframe - rootframe - traj_info->timeline->numposes, + oldcurrframe - rootframe - traj_info->timeline->numframes, SEL.nodesigma, SEL.nodeheight)) { - *currframe = oldcurrframe - traj_info->timeline->numposes; + *currframe = oldcurrframe - traj_info->timeline->numframes; filter = node_calculate_filter_from_frame_offset( - oldcurrframe - rootframe - traj_info->timeline->numposes, + oldcurrframe - rootframe - traj_info->timeline->numframes, SEL.nodesigma, SEL.nodeheight); } if(SEL.loop_enabled && filter < node_calculate_filter_from_frame_offset( - oldcurrframe - rootframe + traj_info->timeline->numposes, + oldcurrframe - rootframe + traj_info->timeline->numframes, SEL.nodesigma, SEL.nodeheight)) { - *currframe = oldcurrframe + traj_info->timeline->numposes; + *currframe = oldcurrframe + traj_info->timeline->numframes; } *frame_offset = *currframe - rootframe; diff --git a/src/overlay.c b/src/overlay.c index 24917a5..5951ad6 100644 --- a/src/overlay.c +++ b/src/overlay.c @@ -198,10 +198,10 @@ void overlay_set_time_and_frame(traj_info_t* traj_info, int frame) { float result; - frame = timeline_make_frame_safe(frame, traj_info->timeline->numposes); + frame = timeline_make_frame_safe(frame, traj_info->timeline->numframes); result = frame; - result /= traj_info->timeline->numposes; + result /= traj_info->timeline->numframes; result *= traj_info->timeline->duration; OV.sec = result; diff --git a/src/simulate.c b/src/simulate.c index 5a22d6d..80dd832 100644 --- a/src/simulate.c +++ b/src/simulate.c @@ -162,7 +162,7 @@ void reset_traj_info() traj_info.selection.joint_cycle_list_size = sizeof(joint_cycle_list); traj_info.selection.joint_cycle_list_index = sizeof(joint_cycle_list)-1; traj_info.selection.joint_cycle_list = malloc(sizeof(joint_cycle_list)); - traj_info.selection.loop_enabled = 0; + traj_info.selection.loop_enabled = 1; memcpy(traj_info.selection.joint_cycle_list,joint_cycle_list,sizeof(joint_cycle_list)); diff --git a/src/timeline.c b/src/timeline.c index 67aadb3..ba3f46b 100644 --- a/src/timeline.c +++ b/src/timeline.c @@ -37,11 +37,11 @@ uint32_t timeline_fill_full_traj_state_array(traj_info_t* traj_info, uint8_t** b return readsofar; } -int timeline_make_frame_safe(int frame, int numposes) +int timeline_make_frame_safe(int frame, int numframes) { while(frame < 0) - frame += numposes; - frame %= numposes; + frame += numframes; + frame %= numframes; return frame; } @@ -90,7 +90,7 @@ void timeiline_init_from_input_file(traj_info_t* traj_info) free(fulls); - traj_info->timeline->numposes = bytecount * loopcount; + traj_info->timeline->numframes = bytecount * loopcount; traj_info->timeline->next = NULL; traj_info->timeline->prev = NULL; traj_info->timeline->node_type = NODE_NONE; @@ -123,7 +123,7 @@ void filename_replace_dots(char* filename) } } -void timeline_export_to_file(traj_info_t* traj_info, full_traj_state_t* fulls, int numposes) +void timeline_export_to_file(traj_info_t* traj_info, full_traj_state_t* fulls, int numframes) { char filename[256]; char infilename[256]; @@ -145,7 +145,7 @@ void timeline_export_to_file(traj_info_t* traj_info, full_traj_state_t* fulls, i } // printf("bytes: %d, doubles: %f\n", sizeof(full_traj_state_t), (sizeof(full_traj_state_t)+0.0)/sizeof(double)); - fwrite(fulls, sizeof(full_traj_state_t), numposes, outfile); + fwrite(fulls, sizeof(full_traj_state_t), numframes, outfile); fflush(outfile); fclose(outfile); } @@ -158,14 +158,14 @@ void timeline_export(traj_info_t* traj_info, timeline_t* timeline) full_traj_state_t* membuf; savestackptr = traj_info->d->pstack; - numDubsNeeded = timeline->numposes * sizeof(full_traj_state_t); + numDubsNeeded = timeline->numframes * sizeof(full_traj_state_t); numDubsNeeded /= sizeof(double); membuf = (full_traj_state_t*) mj_stackAlloc(traj_info->d, mju_ceil(numDubsNeeded)); - for(i = 0; i < timeline->numposes; i++) + for(i = 0; i < timeline->numframes; i++) { - membuf[i].time = (timeline->duration / (timeline->numposes-1)) * i; + membuf[i].time = (timeline->duration / (timeline->numframes-1)) * i; mju_copy(membuf[i].qpos, timeline->qposes[i].q, 35); mju_zero(membuf[i].qvel, 32); mju_zero(membuf[i].torque, 10); @@ -173,7 +173,7 @@ void timeline_export(traj_info_t* traj_info, timeline_t* timeline) mju_zero(membuf[i].mvel, 10); } - timeline_export_to_file(traj_info, membuf, timeline->numposes); + timeline_export_to_file(traj_info, membuf, timeline->numframes); traj_info->d->pstack = savestackptr; } @@ -185,9 +185,9 @@ timeline_t* timeline_init_with_single_pose(qpos_t* qpos, timeline_t* xcopy) int i; dest = malloc(sizeof(timeline_t)); - dest->qposes = malloc(sizeof(qpos_t) * xcopy->numposes); + dest->qposes = malloc(sizeof(qpos_t) * xcopy->numframes); - for(i = 0; i < xcopy->numposes; i++) + for(i = 0; i < xcopy->numframes; i++) { mju_copy(dest->qposes[i].q, qpos->q, CASSIE_QPOS_SIZE); mju_copy(dest->qposes[i].q, xcopy->qposes[i].q, 1); @@ -195,15 +195,13 @@ timeline_t* timeline_init_with_single_pose(qpos_t* qpos, timeline_t* xcopy) dest->next = NULL; dest->prev = NULL; - dest->numposes = xcopy->numposes; + dest->numframes = xcopy->numframes; dest->node_type = NODE_NONE; return dest; } - - timeline_t* timeline_loop(timeline_t* ref, int loopcount) { timeline_t* dest; @@ -212,22 +210,22 @@ timeline_t* timeline_loop(timeline_t* ref, int loopcount) int big; int start; - qposbytecount = sizeof(qpos_t) * ref->numposes * loopcount; + qposbytecount = sizeof(qpos_t) * ref->numframes * loopcount; dest = malloc(sizeof(timeline_t)); dest->qposes = malloc(qposbytecount); - for(i = 0; i < ref->numposes * loopcount; i++) + for(i = 0; i < ref->numframes * loopcount; i++) { mju_copy(dest->qposes[i].q, - ref->qposes[i % ref->numposes].q, + ref->qposes[i % ref->numframes].q, CASSIE_QPOS_SIZE); } for(big = 1; big <= loopcount-1; big++) { - start = ref->numposes * big; - for(i = start; i < start + ref->numposes; i++) + start = ref->numframes * big; + for(i = start; i < start + ref->numframes; i++) { mju_add(dest->qposes[i].q, dest->qposes[i].q, @@ -238,19 +236,19 @@ timeline_t* timeline_loop(timeline_t* ref, int loopcount) dest->next = NULL; dest->prev = NULL; - dest->numposes = ref->numposes * loopcount; + dest->numframes = ref->numframes * loopcount; dest->duration = ref->duration; dest->node_type = NODE_NONE; return dest; } -timeline_t* timeline_truncate(timeline_t* ref, int numposes) +timeline_t* timeline_truncate(timeline_t* ref, int numframes) { timeline_t* dest; int qposbytecount; - qposbytecount = sizeof(qpos_t) * numposes; + qposbytecount = sizeof(qpos_t) * numframes; dest = malloc(sizeof(timeline_t)); dest->qposes = malloc(qposbytecount); @@ -258,7 +256,7 @@ timeline_t* timeline_truncate(timeline_t* ref, int numposes) memcpy(dest->qposes, ref->qposes, qposbytecount); dest->next = NULL; dest->prev = NULL; - dest->numposes = numposes; + dest->numframes = numframes; dest->duration = ref->duration; dest->node_type = NODE_NONE; @@ -270,7 +268,7 @@ timeline_t* timeline_duplicate(timeline_t* ref) timeline_t* dest; int qposbytecount; - qposbytecount = sizeof(qpos_t) * ref->numposes; + qposbytecount = sizeof(qpos_t) * ref->numframes; dest = malloc(sizeof(timeline_t)); dest->qposes = malloc(qposbytecount); @@ -278,7 +276,7 @@ timeline_t* timeline_duplicate(timeline_t* ref) memcpy(dest->qposes, ref->qposes, qposbytecount); dest->next = NULL; dest->prev = NULL; - dest->numposes = ref->numposes; + dest->numframes = ref->numframes; dest->duration = ref->duration; dest->node_type = NODE_NONE; @@ -302,7 +300,7 @@ void timeline_free(timeline_t* ref) qpos_t* timeline_get_qposes_from_frame(timeline_t* timeline, int frame) { - frame = timeline_make_frame_safe(frame, timeline->numposes); + frame = timeline_make_frame_safe(frame, timeline->numframes); return timeline->qposes + frame ; } @@ -323,7 +321,7 @@ void timeline_set_qposes_to_pose_frame(traj_info_t* traj_info, timeline_t* timel if(!timeline) panic(); - frame = timeline_make_frame_safe(frame, timeline->numposes); + frame = timeline_make_frame_safe(frame, timeline->numframes); timeline_set_mj_qpose(traj_info, timeline->qposes + frame); } From 39af67d8f1f7f71318ad8b4d10a840a2d31c2fe0 Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Wed, 29 Aug 2018 15:45:50 -0700 Subject: [PATCH 04/19] truncate -> noloop and rewrite timeline.h --- src/node.c | 2 +- src/timeline.c | 33 +++++++++++++-------------------- src/timeline.h | 37 +++++++++++++++++++++++++++---------- 3 files changed, 41 insertions(+), 31 deletions(-) diff --git a/src/node.c b/src/node.c index edec797..2ecf408 100644 --- a/src/node.c +++ b/src/node.c @@ -337,7 +337,7 @@ void node_perform_pert( failed = 0; timeline_old = traj_info->timeline; - timeline_new = timeline_truncate(timeline_old, timeline_old->numframes/2); + timeline_new = timeline_noloop(timeline_old); rootframe = timeline_make_frame_safe(rootframe, timeline_new->numframes); node_calc_frame_lowhigh( &low_frame, diff --git a/src/timeline.c b/src/timeline.c index ba3f46b..a94644d 100644 --- a/src/timeline.c +++ b/src/timeline.c @@ -90,6 +90,7 @@ void timeiline_init_from_input_file(traj_info_t* traj_info) free(fulls); + traj_info->timeline->numnoloopframes = bytecount; traj_info->timeline->numframes = bytecount * loopcount; traj_info->timeline->next = NULL; traj_info->timeline->prev = NULL; @@ -197,7 +198,7 @@ timeline_t* timeline_init_with_single_pose(qpos_t* qpos, timeline_t* xcopy) dest->prev = NULL; dest->numframes = xcopy->numframes; dest->node_type = NODE_NONE; - + dest->numnoloopframes = xcopy->numnoloopframes; return dest; } @@ -210,22 +211,22 @@ timeline_t* timeline_loop(timeline_t* ref, int loopcount) int big; int start; - qposbytecount = sizeof(qpos_t) * ref->numframes * loopcount; + qposbytecount = sizeof(qpos_t) * ref->numnoloopframes * loopcount; dest = malloc(sizeof(timeline_t)); dest->qposes = malloc(qposbytecount); - for(i = 0; i < ref->numframes * loopcount; i++) + for(i = 0; i < ref->numnoloopframes * loopcount; i++) { mju_copy(dest->qposes[i].q, - ref->qposes[i % ref->numframes].q, + ref->qposes[i % ref->numnoloopframes].q, CASSIE_QPOS_SIZE); } for(big = 1; big <= loopcount-1; big++) { - start = ref->numframes * big; - for(i = start; i < start + ref->numframes; i++) + start = ref->numnoloopframes * big; + for(i = start; i < start + ref->numnoloopframes; i++) { mju_add(dest->qposes[i].q, dest->qposes[i].q, @@ -237,18 +238,19 @@ timeline_t* timeline_loop(timeline_t* ref, int loopcount) dest->next = NULL; dest->prev = NULL; dest->numframes = ref->numframes * loopcount; + dest->numnoloopframes = ref->numnoloopframes; dest->duration = ref->duration; dest->node_type = NODE_NONE; return dest; } -timeline_t* timeline_truncate(timeline_t* ref, int numframes) +timeline_t* timeline_noloop(timeline_t* ref) { timeline_t* dest; int qposbytecount; - qposbytecount = sizeof(qpos_t) * numframes; + qposbytecount = sizeof(qpos_t) * ref->numnoloopframes; dest = malloc(sizeof(timeline_t)); dest->qposes = malloc(qposbytecount); @@ -256,7 +258,8 @@ timeline_t* timeline_truncate(timeline_t* ref, int numframes) memcpy(dest->qposes, ref->qposes, qposbytecount); dest->next = NULL; dest->prev = NULL; - dest->numframes = numframes; + dest->numframes = ref->numnoloopframes; + dest->numnoloopframes = ref->numnoloopframes; dest->duration = ref->duration; dest->node_type = NODE_NONE; @@ -277,6 +280,7 @@ timeline_t* timeline_duplicate(timeline_t* ref) dest->next = NULL; dest->prev = NULL; dest->numframes = ref->numframes; + dest->numnoloopframes = ref->numnoloopframes; dest->duration = ref->duration; dest->node_type = NODE_NONE; @@ -310,17 +314,9 @@ void timeline_set_mj_qpose(traj_info_t* traj_info, qpos_t* desired) mju_copy(traj_info->d->qpos, desired->q, CASSIE_QPOS_SIZE); } -void panic() -{ - fprintf(stderr, "PANIC!\n"); - exit(1); -} void timeline_set_qposes_to_pose_frame(traj_info_t* traj_info, timeline_t* timeline, int frame) { - if(!timeline) - panic(); - frame = timeline_make_frame_safe(frame, timeline->numframes); timeline_set_mj_qpose(traj_info, timeline->qposes + frame); @@ -329,9 +325,6 @@ void timeline_set_qposes_to_pose_frame(traj_info_t* traj_info, timeline_t* timel void timeline_overwrite_frame_using_curr_pose(traj_info_t* traj_info, timeline_t* timeline, int frame) { qpos_t* qposes; - - if(!timeline) - panic(); qposes = timeline_get_qposes_from_frame(timeline, frame); diff --git a/src/timeline.h b/src/timeline.h index a050847..e56cddb 100644 --- a/src/timeline.h +++ b/src/timeline.h @@ -21,20 +21,37 @@ typedef struct _full_traj_state_t_ full_traj_state_t; #include "timeline.h" - -timeline_t* timeline_init_with_single_pose(qpos_t* qpos, timeline_t* xcopy); -timeline_t* timeline_loop(timeline_t* ref, int loopcount); -timeline_t* timeline_truncate(timeline_t* ref, int numposes); +void timeiline_init_from_input_file(traj_info_t* traj_info); timeline_t* timeline_duplicate(timeline_t* ref); -void timeline_safe_link(timeline_t* next, timeline_t* prev); +void timeline_export_to_file(traj_info_t* traj_info, + full_traj_state_t* fulls, + int numframes); +void timeline_export(traj_info_t* traj_info, + timeline_t* timeline); +uint32_t timeline_fill_full_traj_state_array(traj_info_t* traj_info, + uint8_t** buf); void timeline_free(timeline_t* ref); -qpos_t* timeline_get_qposes_from_frame(timeline_t* timeline, int frame); -void timeline_set_qposes_to_pose_frame(traj_info_t* traj_info, timeline_t* timeline, int frame); -void timeline_overwrite_frame_using_curr_pose(traj_info_t* traj_info, timeline_t* timeline, int frame); -int timeline_make_frame_safe(int frame, int numposes); int timeline_get_frame_from_time(traj_info_t* traj_info); +qpos_t* timeline_get_qposes_from_frame(timeline_t* timeline, + int frame); +timeline_t* timeline_init_with_single_pose(qpos_t* qpos, + timeline_t* xcopy); +timeline_t* timeline_loop(timeline_t* ref, + int loopcount); +int timeline_make_frame_safe(int frame, + int numframes); +timeline_t* timeline_noloop(timeline_t* ref); +void timeline_overwrite_frame_using_curr_pose(traj_info_t* traj_info, + timeline_t* timeline, + int frame); +void timeline_safe_link(timeline_t* next, + timeline_t* prev); +void timeline_set_mj_qpose(traj_info_t* traj_info, + qpos_t* desired); +void timeline_set_qposes_to_pose_frame(traj_info_t* traj_info, + timeline_t* timeline, + int frame); void timeline_update_mj_poses_from_realtime(traj_info_t* traj_info); -void timeline_export(traj_info_t* traj_info, timeline_t* timeline); #endif From c851777d089f56b6d71903060bfd0289085547e0 Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Wed, 29 Aug 2018 15:46:48 -0700 Subject: [PATCH 05/19] rewrite timeline again --- src/timeline.h | 36 +++++++++++------------------------- 1 file changed, 11 insertions(+), 25 deletions(-) diff --git a/src/timeline.h b/src/timeline.h index e56cddb..f25862f 100644 --- a/src/timeline.h +++ b/src/timeline.h @@ -23,34 +23,20 @@ typedef struct _full_traj_state_t_ full_traj_state_t; void timeiline_init_from_input_file(traj_info_t* traj_info); timeline_t* timeline_duplicate(timeline_t* ref); -void timeline_export_to_file(traj_info_t* traj_info, - full_traj_state_t* fulls, - int numframes); -void timeline_export(traj_info_t* traj_info, - timeline_t* timeline); -uint32_t timeline_fill_full_traj_state_array(traj_info_t* traj_info, - uint8_t** buf); +void timeline_export_to_file(traj_info_t* traj_info, full_traj_state_t* fulls, int numframes); +void timeline_export(traj_info_t* traj_info, timeline_t* timeline); +uint32_t timeline_fill_full_traj_state_array(traj_info_t* traj_info, uint8_t** buf); void timeline_free(timeline_t* ref); int timeline_get_frame_from_time(traj_info_t* traj_info); -qpos_t* timeline_get_qposes_from_frame(timeline_t* timeline, - int frame); -timeline_t* timeline_init_with_single_pose(qpos_t* qpos, - timeline_t* xcopy); -timeline_t* timeline_loop(timeline_t* ref, - int loopcount); -int timeline_make_frame_safe(int frame, - int numframes); +qpos_t* timeline_get_qposes_from_frame(timeline_t* timeline, int frame); +timeline_t* timeline_init_with_single_pose(qpos_t* qpos, timeline_t* xcopy); +timeline_t* timeline_loop(timeline_t* ref, int loopcount); +int timeline_make_frame_safe(int frame, int numframes); timeline_t* timeline_noloop(timeline_t* ref); -void timeline_overwrite_frame_using_curr_pose(traj_info_t* traj_info, - timeline_t* timeline, - int frame); -void timeline_safe_link(timeline_t* next, - timeline_t* prev); -void timeline_set_mj_qpose(traj_info_t* traj_info, - qpos_t* desired); -void timeline_set_qposes_to_pose_frame(traj_info_t* traj_info, - timeline_t* timeline, - int frame); +void timeline_overwrite_frame_using_curr_pose(traj_info_t* traj_info, timeline_t* timeline, int frame); +void timeline_safe_link(timeline_t* next, timeline_t* prev); +void timeline_set_mj_qpose(traj_info_t* traj_info, qpos_t* desired); +void timeline_set_qposes_to_pose_frame(traj_info_t* traj_info, timeline_t* timeline, int frame); void timeline_update_mj_poses_from_realtime(traj_info_t* traj_info); From 354e63217a3f54826d9bee0119c249d643396246 Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Wed, 29 Aug 2018 15:54:49 -0700 Subject: [PATCH 06/19] #29 in progress and works for positional, a-scaled --- src/node.c | 31 ++++++++++++++++++------------- src/timeline.c | 2 +- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/node.c b/src/node.c index 2ecf408..8eff36c 100644 --- a/src/node.c +++ b/src/node.c @@ -140,24 +140,27 @@ void node_calclate_global_target_using_transformation_type( { double filter; v3_t body_init_xpos; - double plus,minus; + int i; + double temp; filter = node_calculate_filter_from_frame_offset( frame_offset, SEL.nodesigma, SEL.nodeheight); - #warning "fix this" - - plus = node_calculate_filter_from_frame_offset( - frame_offset + traj_info->timeline->numframes/2, - SEL.nodesigma, - SEL.nodeheight); - minus = node_calculate_filter_from_frame_offset( - frame_offset - traj_info->timeline->numframes/2, - SEL.nodesigma, - SEL.nodeheight); - filter = mju_max(mju_max(plus,minus),filter); + for(i = 0; i * traj_info->timeline->numnoloopframes < traj_info->timeline->numframes; i++) + { + temp = node_calculate_filter_from_frame_offset( + frame_offset + i * traj_info->timeline->numnoloopframes, + SEL.nodesigma, + SEL.nodeheight); + filter = mju_max(temp,filter); + temp = node_calculate_filter_from_frame_offset( + frame_offset - i * traj_info->timeline->numnoloopframes, + SEL.nodesigma, + SEL.nodeheight); + filter = mju_max(temp,filter); + } body_init_xpos = node_get_body_xpos_by_frame( traj_info, @@ -475,7 +478,9 @@ void node_perform_pert( (iktimedelta/1000000.0), 1000.0*params->ik_accuracy_cutoff); - timeline_final = timeline_loop(timeline_new,2); + timeline_final = timeline_loop( + timeline_new, + mju_round(timeline_old->numframes/timeline_old->numnoloopframes)); timeline_free(timeline_new); timeline_new = timeline_final; timeline_safe_link(timeline_new, timeline_old); diff --git a/src/timeline.c b/src/timeline.c index a94644d..cf5d65a 100644 --- a/src/timeline.c +++ b/src/timeline.c @@ -54,7 +54,7 @@ void timeiline_init_from_input_file(traj_info_t* traj_info) int start; int loopcount; - loopcount = 2; + loopcount = 3; bytecount = timeline_fill_full_traj_state_array(traj_info, (uint8_t**) &fulls); bytecount /= sizeof(full_traj_state_t); From 93c7ee0ac34285ac58e753c1e6b11c9e4d2150ba Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Wed, 29 Aug 2018 16:03:31 -0700 Subject: [PATCH 07/19] b scaling can be perted properly --- src/node.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/node.c b/src/node.c index 8eff36c..a9f0f29 100644 --- a/src/node.c +++ b/src/node.c @@ -352,7 +352,9 @@ void node_perform_pert( init_time = traj_calculate_runtime_micros(traj_info); mju_copy3( global_body_init_xpos_at_rootframe, - node_get_body_xpos_curr(traj_info, body_id)); + node_get_body_xpos_by_frame(traj_info, timeline_new, rootframe, body_id)); + + node_calclate_global_target_using_transformation_type( traj_info, @@ -779,6 +781,7 @@ void node_scale_visually_positional( body_id, node_id); + rootframe = get_frame_from_node_body_id(traj_info, traj_info->timeline, node_id); From 4698a04a01e8a51472e1832b0179e03783a049e5 Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Wed, 29 Aug 2018 16:23:44 -0700 Subject: [PATCH 08/19] not sure what changed --- src/node.c | 2 -- src/simulate.c | 4 +++- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/node.c b/src/node.c index a9f0f29..a173e8c 100644 --- a/src/node.c +++ b/src/node.c @@ -354,8 +354,6 @@ void node_perform_pert( global_body_init_xpos_at_rootframe, node_get_body_xpos_by_frame(traj_info, timeline_new, rootframe, body_id)); - - node_calclate_global_target_using_transformation_type( traj_info, timeline_new, diff --git a/src/simulate.c b/src/simulate.c index 80dd832..0e2c199 100644 --- a/src/simulate.c +++ b/src/simulate.c @@ -62,7 +62,7 @@ double window2buffer = 1; // framebuffersize / windowsize (for scaled // help strings const char help_title[] = -"More Help\n" +"Cycle Help\n" "Option\n" "Info\n" "Full screen\n" @@ -94,6 +94,7 @@ const char help_content[] = ". /"; const char help2_title[] = +"Cycle Help\n" "Fwd/Bk\n" "Fwd/Bk 20\n" "Advance Nodes\n" @@ -109,6 +110,7 @@ const char help2_title[] = ; const char help2_content[] = +"F1\n" "R/L arrows\n" "U/D arrows\n" "Ctrl L/R\n" From d69dc9d4fb81459bd85740e03adffba82855a6b1 Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Wed, 29 Aug 2018 17:17:13 -0700 Subject: [PATCH 09/19] progress on looped bscaling, visually --- src/node.c | 45 +++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 43 insertions(+), 2 deletions(-) diff --git a/src/node.c b/src/node.c index a173e8c..07e1181 100644 --- a/src/node.c +++ b/src/node.c @@ -127,6 +127,37 @@ void node_calculate_arbitrary_target_using_scale_type( traj_info->d->pstack = stack_mark; } +void node_gimme_target_friendly_rf_init_body_xpos( + traj_info_t* traj_info, + timeline_t* timeline, + v3_t fixed_body_init_xpos_at_rootframe, + v3_t global_body_init_xpos_at_rootframe, + int rootframe, + int frame_offset, + cassie_body_id_t body_id) +{ + double pelvis_start[3]; + double pelvis_end[3]; + double pelvis_delta[3]; + int numdeltas; + + numdeltas = mju_round((frame_offset + 0.0) / (timeline->numnoloopframes + 0.0)); + mju_copy3(pelvis_start, node_get_body_xpos_by_frame( + traj_info, + timeline, + 0, + node_get_cassie_id_from_index(1))); + mju_copy3(pelvis_end, node_get_body_xpos_by_frame( + traj_info, + timeline, + timeline->numnoloopframes-1, + node_get_cassie_id_from_index(1))); + mju_sub3(pelvis_delta, pelvis_end, pelvis_start); + mju_addScl3(fixed_body_init_xpos_at_rootframe, + global_body_init_xpos_at_rootframe, + pelvis_delta, + numdeltas); +} void node_calclate_global_target_using_transformation_type( traj_info_t* traj_info, @@ -142,6 +173,7 @@ void node_calclate_global_target_using_transformation_type( v3_t body_init_xpos; int i; double temp; + double fixed_body_init_xpos_at_rootframe[3]; filter = node_calculate_filter_from_frame_offset( frame_offset, @@ -162,18 +194,27 @@ void node_calclate_global_target_using_transformation_type( filter = mju_max(temp,filter); } + node_gimme_target_friendly_rf_init_body_xpos( + traj_info, + timeline, + fixed_body_init_xpos_at_rootframe, + global_body_init_xpos_at_rootframe, + rootframe, + frame_offset, + body_id); + body_init_xpos = node_get_body_xpos_by_frame( traj_info, timeline, rootframe + frame_offset, - body_id); + body_id); node_calculate_arbitrary_target_using_scale_type( traj_info, global_body_target_xpos, rootframe_transform_vector, body_init_xpos, - global_body_init_xpos_at_rootframe, + fixed_body_init_xpos_at_rootframe, 3, filter); } From f16a913e6adc3a50c3065ab90031b70d96c81bed Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Thu, 30 Aug 2018 09:46:40 -0700 Subject: [PATCH 10/19] missing --- src/main.h | 1 + src/node.c | 2 +- src/timeline.c | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main.h b/src/main.h index 4ebbef0..4d9db13 100644 --- a/src/main.h +++ b/src/main.h @@ -13,6 +13,7 @@ #define FILENAME_STEP_DATA "stepdata.bin" //used in simulate.c : reset_traj_info() #define DECOR_BUF_SIZE 400 #define IK_STEP_CUTOFF 1500 +#define LOOP_TRAJECTORY 1 struct _qpos_t_ { diff --git a/src/node.c b/src/node.c index 07e1181..97b2d0f 100644 --- a/src/node.c +++ b/src/node.c @@ -502,7 +502,7 @@ void node_perform_pert( &ik_iter_total); } - if(frame_offset > 1 && ik_iter_total > (.95 * IK_STEP_CUTOFF * (2*frame_offset + 1))) + if(frame_offset > 1 && ik_iter_total > (.6 * IK_STEP_CUTOFF * (2*frame_offset + 1))) { printf("TOO HARD. ABORTING. \n"); failed = 1; diff --git a/src/timeline.c b/src/timeline.c index cf5d65a..b0bdbfe 100644 --- a/src/timeline.c +++ b/src/timeline.c @@ -54,7 +54,7 @@ void timeiline_init_from_input_file(traj_info_t* traj_info) int start; int loopcount; - loopcount = 3; + loopcount = LOOP_TRAJECTORY; bytecount = timeline_fill_full_traj_state_array(traj_info, (uint8_t**) &fulls); bytecount /= sizeof(full_traj_state_t); From 5025d0567616687234c87e5b51e198ec50f5f464 Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Thu, 30 Aug 2018 09:57:33 -0700 Subject: [PATCH 11/19] single loop works well for bscaling --- src/main.h | 2 +- src/node.c | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main.h b/src/main.h index 4d9db13..b5f6c3e 100644 --- a/src/main.h +++ b/src/main.h @@ -13,7 +13,7 @@ #define FILENAME_STEP_DATA "stepdata.bin" //used in simulate.c : reset_traj_info() #define DECOR_BUF_SIZE 400 #define IK_STEP_CUTOFF 1500 -#define LOOP_TRAJECTORY 1 +#define LOOP_TRAJECTORY 2 struct _qpos_t_ { diff --git a/src/node.c b/src/node.c index 97b2d0f..f4e6dee 100644 --- a/src/node.c +++ b/src/node.c @@ -175,6 +175,8 @@ void node_calclate_global_target_using_transformation_type( double temp; double fixed_body_init_xpos_at_rootframe[3]; + // printf("fo: %d \n", frame_offset); + filter = node_calculate_filter_from_frame_offset( frame_offset, SEL.nodesigma, @@ -194,6 +196,9 @@ void node_calclate_global_target_using_transformation_type( filter = mju_max(temp,filter); } + frame_offset = timeline_make_frame_safe( rootframe + frame_offset, + timeline->numnoloopframes) - rootframe; + node_gimme_target_friendly_rf_init_body_xpos( traj_info, timeline, From fd19cbc6bdf1e7868defc040b8525473cd79efcb Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Thu, 30 Aug 2018 10:07:30 -0700 Subject: [PATCH 12/19] appears only multiple loops is breaking --- src/main.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.h b/src/main.h index b5f6c3e..4d9db13 100644 --- a/src/main.h +++ b/src/main.h @@ -13,7 +13,7 @@ #define FILENAME_STEP_DATA "stepdata.bin" //used in simulate.c : reset_traj_info() #define DECOR_BUF_SIZE 400 #define IK_STEP_CUTOFF 1500 -#define LOOP_TRAJECTORY 2 +#define LOOP_TRAJECTORY 1 struct _qpos_t_ { From d29633fa041f05e833af30bc16893eaea4279f7e Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Thu, 30 Aug 2018 11:49:27 -0700 Subject: [PATCH 13/19] this doesnt fix this bscale looping but i tried --- src/node.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/node.c b/src/node.c index f4e6dee..e521060 100644 --- a/src/node.c +++ b/src/node.c @@ -197,7 +197,7 @@ void node_calclate_global_target_using_transformation_type( } frame_offset = timeline_make_frame_safe( rootframe + frame_offset, - timeline->numnoloopframes) - rootframe; + timeline->numframes) - rootframe; node_gimme_target_friendly_rf_init_body_xpos( traj_info, From 93a80f6a7b9a671acae746e0740d86db12daa411 Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Thu, 30 Aug 2018 14:02:39 -0700 Subject: [PATCH 14/19] further work on #29, only needto fix visible scaling --- src/main.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.h b/src/main.h index 4d9db13..b5f6c3e 100644 --- a/src/main.h +++ b/src/main.h @@ -13,7 +13,7 @@ #define FILENAME_STEP_DATA "stepdata.bin" //used in simulate.c : reset_traj_info() #define DECOR_BUF_SIZE 400 #define IK_STEP_CUTOFF 1500 -#define LOOP_TRAJECTORY 1 +#define LOOP_TRAJECTORY 2 struct _qpos_t_ { From d64f974fcdfb9a216d2660e3ddb64b5a63ff0b22 Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Fri, 31 Aug 2018 10:17:06 -0700 Subject: [PATCH 15/19] fixed merge conflicts when mergeing docs in for REAL --- src/node.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/node.c b/src/node.c index 71ab4c1..5e3eb63 100644 --- a/src/node.c +++ b/src/node.c @@ -825,9 +825,7 @@ void node_scale_visually_positional( body_id, node_id); - rootframe = node_get_frame_from_node_body_id(traj_info, - nodedocs traj_info->timeline, node_id); mju_copy3(global_body_init_xpos_at_rootframe, From 93e9b1c2e846326afd3a1ef6520ae51bd5c7016f Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Fri, 31 Aug 2018 11:22:57 -0700 Subject: [PATCH 16/19] progress on jointnum --- src/node.c | 37 +++++++++++++++++++++++++++++++++---- 1 file changed, 33 insertions(+), 4 deletions(-) diff --git a/src/node.c b/src/node.c index 5e3eb63..ea57afc 100644 --- a/src/node.c +++ b/src/node.c @@ -555,16 +555,22 @@ void node_dropped_jointmove( double rootframe_init; double filter; double jointdiff; + double temp; + int i; + int frame_offset; timeline_t* timeline_old; timeline_t* timeline_new; + timeline_t* timeline_final; timeline_old = traj_info->timeline; - timeline_new = timeline_duplicate(timeline_old); + timeline_new = timeline_noloop(timeline_old); rootframe = node_get_frame_from_node_body_id(traj_info, timeline_new, node_id); + // rootframe = timeline_make_frame_safe(rootframe, timeline_new->numnoloopframes); + timeline_set_qposes_to_pose_frame( traj_info, timeline_old, @@ -576,13 +582,30 @@ void node_dropped_jointmove( jointdiff = node_caluclate_jointdiff(traj_info, node_get_body_xpos_curr(traj_info, body_id)); - for (frame = 0; frame < timeline_new->numframes; frame++) + + + for (frame = 0; frame < timeline_new->numnoloopframes; frame++) { + frame_offset = frame - rootframe; filter = node_calculate_filter_from_frame_offset( - frame - rootframe, + frame_offset, SEL.nodesigma, SEL.nodeheight); + for(i = 0; SEL.loop_enabled && i * traj_info->timeline->numnoloopframes <= traj_info->timeline->numframes; i++) + { + temp = node_calculate_filter_from_frame_offset( + frame_offset + i * traj_info->timeline->numnoloopframes, + SEL.nodesigma, + SEL.nodeheight); + filter = mju_max(temp,filter); + temp = node_calculate_filter_from_frame_offset( + frame_offset - i * traj_info->timeline->numnoloopframes, + SEL.nodesigma, + SEL.nodeheight); + filter = mju_max(temp,filter); + } + node_calculate_arbitrary_target_using_scale_type( traj_info, timeline_new->qposes[frame].q + GETJOINTNUM, @@ -593,8 +616,14 @@ void node_dropped_jointmove( filter); } - timeline_safe_link(timeline_new, timeline_old); + timeline_final = timeline_loop( + timeline_new, + mju_round(timeline_old->numframes/timeline_old->numnoloopframes)); + timeline_free(timeline_new); + timeline_new = timeline_final; + timeline_safe_link(timeline_new, timeline_old); traj_info->timeline = timeline_new; + timeline_new->node_type = NODE_JOINTMOVE; node_position_initial_using_cassie_body(traj_info, body_id); } From f81f293e0463583c1d70079c778711cbd8f81097 Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Fri, 31 Aug 2018 12:12:25 -0700 Subject: [PATCH 17/19] fixed thing in control --- src/control.c | 42 +++++++++++++++++++++++++++--------------- 1 file changed, 27 insertions(+), 15 deletions(-) diff --git a/src/control.c b/src/control.c index d80ca6b..0f915ac 100644 --- a/src/control.c +++ b/src/control.c @@ -168,6 +168,33 @@ void control_key_event(traj_info_t* traj_info, int key, int mods) traj_info->time_frozen -= 1000000; if (key == GLFW_KEY_UP) traj_info->time_frozen += 1000000; + if( key==GLFW_KEY_PAGE_UP) + { + SEL.joint_cycle_list_index = (SEL.joint_cycle_list_index + 1) % SEL.joint_cycle_list_size; + if(SEL.node_type == NODE_JOINTID) + REVISUALIZE; + } + else if( key==GLFW_KEY_PAGE_DOWN) + { + SEL.joint_cycle_list_index = (SEL.joint_cycle_list_index - 1 + SEL.joint_cycle_list_size) % SEL.joint_cycle_list_size; + if(SEL.node_type == NODE_JOINTID) + REVISUALIZE; + } + } + else if (!(*traj_info->paused) && !(mods & GLFW_MOD_CONTROL)) + { + if( key==GLFW_KEY_PAGE_UP) + { + SEL.joint_cycle_list_index = (SEL.joint_cycle_list_index + 1) % SEL.joint_cycle_list_size; + if(SEL.node_type == NODE_JOINTID) + REVISUALIZE; + } + else if( key==GLFW_KEY_PAGE_DOWN) + { + SEL.joint_cycle_list_index = (SEL.joint_cycle_list_index - 1 + SEL.joint_cycle_list_size) % SEL.joint_cycle_list_size; + if(SEL.node_type == NODE_JOINTID) + REVISUALIZE; + } } else if (mods & GLFW_MOD_CONTROL) { @@ -202,21 +229,6 @@ void control_key_event(traj_info_t* traj_info, int key, int mods) traj_info->time_start -= traj_info->time_start*(.2) - traj_time_in_micros()*(.2); } } - else - { - if( key==GLFW_KEY_PAGE_UP) - { - SEL.joint_cycle_list_index = (SEL.joint_cycle_list_index + 1) % SEL.joint_cycle_list_size; - if(SEL.node_type == NODE_JOINTID) - REVISUALIZE; - } - else if( key==GLFW_KEY_PAGE_DOWN) - { - SEL.joint_cycle_list_index = (SEL.joint_cycle_list_index - 1 + SEL.joint_cycle_list_size) % SEL.joint_cycle_list_size; - if(SEL.node_type == NODE_JOINTID) - REVISUALIZE; - } - } if(key== GLFW_KEY_C) { From 4de85f8d0245e0ed21b575eb4eb2b74d841f637e Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Fri, 31 Aug 2018 12:32:25 -0700 Subject: [PATCH 18/19] joint modes only work for one loop --- src/main.h | 2 +- src/node.c | 18 +++++++----------- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/main.h b/src/main.h index b5f6c3e..4d9db13 100644 --- a/src/main.h +++ b/src/main.h @@ -13,7 +13,7 @@ #define FILENAME_STEP_DATA "stepdata.bin" //used in simulate.c : reset_traj_info() #define DECOR_BUF_SIZE 400 #define IK_STEP_CUTOFF 1500 -#define LOOP_TRAJECTORY 2 +#define LOOP_TRAJECTORY 1 struct _qpos_t_ { diff --git a/src/node.c b/src/node.c index ea57afc..ec95c4c 100644 --- a/src/node.c +++ b/src/node.c @@ -563,14 +563,12 @@ void node_dropped_jointmove( timeline_t* timeline_final; timeline_old = traj_info->timeline; - timeline_new = timeline_noloop(timeline_old); + timeline_new = timeline_duplicate(timeline_old); rootframe = node_get_frame_from_node_body_id(traj_info, timeline_new, node_id); - - // rootframe = timeline_make_frame_safe(rootframe, timeline_new->numnoloopframes); - + rootframe = timeline_make_frame_safe(rootframe, timeline_new->numnoloopframes); timeline_set_qposes_to_pose_frame( traj_info, timeline_old, @@ -582,8 +580,6 @@ void node_dropped_jointmove( jointdiff = node_caluclate_jointdiff(traj_info, node_get_body_xpos_curr(traj_info, body_id)); - - for (frame = 0; frame < timeline_new->numnoloopframes; frame++) { frame_offset = frame - rootframe; @@ -616,11 +612,11 @@ void node_dropped_jointmove( filter); } - timeline_final = timeline_loop( - timeline_new, - mju_round(timeline_old->numframes/timeline_old->numnoloopframes)); - timeline_free(timeline_new); - timeline_new = timeline_final; + // timeline_final = timeline_loop( + // timeline_new, + // mju_round(timeline_old->numframes/timeline_old->numnoloopframes)); + // timeline_free(timeline_new); + // timeline_new = timeline_final; timeline_safe_link(timeline_new, timeline_old); traj_info->timeline = timeline_new; timeline_new->node_type = NODE_JOINTMOVE; From 6e6f846f9fb34c7999b3e25d5daac6bc21a721ec Mon Sep 17 00:00:00 2001 From: Kevin Kellar Date: Fri, 31 Aug 2018 12:37:21 -0700 Subject: [PATCH 19/19] rearrange main header --- src/main.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main.h b/src/main.h index 4d9db13..9ab1acc 100644 --- a/src/main.h +++ b/src/main.h @@ -11,9 +11,10 @@ #define XMLNODECOUNT 50 #define NODECOUNT (traj_info->selection.nodecount) #define FILENAME_STEP_DATA "stepdata.bin" //used in simulate.c : reset_traj_info() -#define DECOR_BUF_SIZE 400 +#define LOOP_TRAJECTORY 2 #define IK_STEP_CUTOFF 1500 -#define LOOP_TRAJECTORY 1 + +#define DECOR_BUF_SIZE 400 struct _qpos_t_ { @@ -29,7 +30,11 @@ enum node_type_e NODE_JOINTMOVE }; +#if (LOOP_TRAJECTORY == 1) #define NODE_TYPE_E_COUNT 3 +#else +#define NODE_TYPE_E_COUNT 1 +#endif enum scale_type_e {