Merge pull request #1267 from hsnuhayato/fix_core_dump_in_setTargetPose

[SequencePlayer]fix_core_dump_in_setTargetPose
fkanehiro · Mar 6, 2019 · ea04aa7 · ea04aa7
2 parents ee050df + 630c52b
commit ea04aa7
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 3 deletions.
diff --git a/rtc/SequencePlayer/SequencePlayer.cpp b/rtc/SequencePlayer/SequencePlayer.cpp
@@ -637,9 +637,9 @@ bool SequencePlayer::setTargetPose(const char* gname, const double *xyz, const d
     }
     manip->setMaxIKError(m_error_pos,m_error_rot);
     manip->setMaxIKIteration(m_iteration);
-    std::cerr << "[setTargetPose] Solveing IK with frame" << frame_name << ", Error " << m_error_pos << m_error_rot << ", Iteration " << m_iteration << std::endl;
-    std::cerr << "                Start " << start_p << start_R<< std::endl;
-    std::cerr << "                End   " << end_p << end_R<< std::endl;
+    std::cerr << "[setTargetPose] Solveing IK with frame " << (frame_name? frame_name:"world_frame") << ", Error " << m_error_pos << m_error_rot << ", Iteration " << m_iteration << std::endl;
+    std::cerr << "                Start\n" << start_p << "\n" << start_R<< std::endl;
+    std::cerr << "                End\n" << end_p << "\n" << end_R<< std::endl;
 
     // interpolate & calc ik
     int len = max(((start_p - end_p).norm() / 0.02 ), // 2cm

diff --git a/sample/SampleRobot/samplerobot_sequence_player.py b/sample/SampleRobot/samplerobot_sequence_player.py
@@ -425,6 +425,8 @@ def demoSetTargetPose():
     hcf.seq_svc.waitInterpolationOfGroup('larm');
     pos0 = hcf.getCurrentPosition('LARM_WRIST_R:WAIST')
     rpy0 = hcf.getCurrentRPY('LARM_WRIST_R:WAIST')
+    pos0_world = hcf.getCurrentPosition('LARM_WRIST_R')
+    rpy0_world = hcf.getCurrentRPY('LARM_WRIST_R')
     p0 = list(reset_pose_doc['pos']) # copy
     for i in range(len(larm_pos0)):
         p0[i+19] = larm_pos0[i]
@@ -434,6 +436,8 @@ def demoSetTargetPose():
     hcf.seq_svc.waitInterpolationOfGroup('larm');
     pos1 = hcf.getCurrentPosition('LARM_WRIST_R:WAIST')
     rpy1 = hcf.getCurrentRPY('LARM_WRIST_R:WAIST')
+    pos1_world = hcf.getCurrentPosition('LARM_WRIST_R')
+    rpy1_world = hcf.getCurrentRPY('LARM_WRIST_R')
     p1 = list(reset_pose_doc['pos']) # copy
     for i in range(len(larm_pos1)):
         p1[i+19] = larm_pos1[i]
@@ -452,6 +456,22 @@ def demoSetTargetPose():
     checkJointAngles(p1, 0.01)
     assert(checkServoStateFromLog() is True)
 
+    hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0);
+    hcf.seq_svc.waitInterpolationOfGroup('larm');
+
+    print >> sys.stderr, "   check setTargetPose without giving reference frame"
+    hcf.seq_svc.setTargetPose('larm', pos0_world, rpy0_world, 2.0)
+    hcf.seq_svc.waitInterpolationOfGroup('larm');
+    print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0)
+    checkJointAngles(p0, 0.01)
+
+    clearLogForCheckParameter(5000)
+    hcf.seq_svc.setTargetPose('larm', pos1_world, rpy1_world, 2.0)
+    hcf.seq_svc.waitInterpolationOfGroup('larm');
+    print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1)
+    checkJointAngles(p1, 0.01)
+    assert(checkServoStateFromLog() is True)
+
     print >> sys.stderr, "   check clear clearOfGroup"
     hcf.seq_svc.setTargetPose('larm:WAIST', pos0, rpy0, 2.0)
     time.sleep(0.5)