[ServoController] Fix explicit bugs

[pazeshun]

What is problem

When I moved the right hand of HIRONX using the latest hrpsys including some modifications, the left finger of left hand also moved.
What I commanded is as follows:

$ ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py -- --host hiro014
Python 2.7.12 (default, Oct  8 2019, 14:14:10) 
Type "copyright", "credits" or "license" for more information.

IPython 2.4.1 -- An enhanced Interactive Python.
?         -> Introduction and overview of IPython's features.
%quickref -> Quick reference.
help      -> Python's own help system.
object?   -> Details about 'object', use 'object??' for extra details.
Failed to import pyassimp, see https://github.com/ros-planning/moveit/issues/86 for more info
configuration ORB with hiro014:15005
[hrpsys.py] waiting ModelLoader
[hrpsys.py] start hrpsys
[hrpsys.py] finding RTCManager and RobotHardware
[hrpsys.py]  waitForRTCManagerAndRoboHardware has renamed to waitForRTCManagerAndRoboHardware: Please update your code
[hrpsys.py] wait for RTCmanager : None
[hrpsys.py] wait for RobotHardware0 : <hrpsys.rtm.RTcomponent instance at 0x7f11548695f0> ( timeout 0 < 10)
[hrpsys.py] findComps -> RobotHardware0 : <hrpsys.rtm.RTcomponent instance at 0x7f11548695f0> isActive? = False 
[hrpsys.py] simulation_mode : False
[hrpsys.py]  Hrpsys controller version info: 
[hrpsys.py]    ms =  <hrpsys.rtm.RTCmanager instance at 0x7f115486e1b8>
[hrpsys.py]    ref =  <RTM._objref_Manager instance at 0x7f115486e0e0>
[hrpsys.py]    version  =  315.15.0
[hrpsys.py]   bodyinfo URL = file:///opt/jsk/etc/HIRONX/model/main.wrl
[hrpsys.py] no hrpsys components found running. creating now
[hrpsys.py]   eval : [self.seq, self.seq_svc, self.seq_version] = self.createComp("SequencePlayer","seq")
[hrpsys.py] create Comp -> SequencePlayer : <hrpsys.rtm.RTcomponent instance at 0x7f1154884c68> (315.15.0)
[hrpsys.py] create CompSvc -> SequencePlayer Service : <OpenHRP._objref_SequencePlayerService instance at 0x7f1154888830>
[hrpsys.py]   eval : [self.sh, self.sh_svc, self.sh_version] = self.createComp("StateHolder","sh")
[hrpsys.py] create Comp -> StateHolder : <hrpsys.rtm.RTcomponent instance at 0x7f1154879ab8> (315.15.0)
[hrpsys.py] create CompSvc -> StateHolder Service : <OpenHRP._objref_StateHolderService instance at 0x7f11548a7b48>
[hrpsys.py]   eval : [self.fk, self.fk_svc, self.fk_version] = self.createComp("ForwardKinematics","fk")
[hrpsys.py] create Comp -> ForwardKinematics : <hrpsys.rtm.RTcomponent instance at 0x7f11548882d8> (315.15.0)
[hrpsys.py] create CompSvc -> ForwardKinematics Service : <OpenHRP._objref_ForwardKinematicsService instance at 0x7f115487bc20>
[hrpsys.py]   eval : [self.ic, self.ic_svc, self.ic_version] = self.createComp("ImpedanceController","ic")
[hrpsys.py] create Comp -> ImpedanceController : <hrpsys.rtm.RTcomponent instance at 0x7f1154869ea8> (315.15.0)
[hrpsys.py] create CompSvc -> ImpedanceController Service : <OpenHRP._objref_ImpedanceControllerService instance at 0x7f1154895b00>
[hrpsys.py]   eval : [self.el, self.el_svc, self.el_version] = self.createComp("SoftErrorLimiter","el")
[hrpsys.py] create Comp -> SoftErrorLimiter : <hrpsys.rtm.RTcomponent instance at 0x7f115486e950> (315.15.0)
[hrpsys.py] create CompSvc -> SoftErrorLimiter Service : <OpenHRP._objref_SoftErrorLimiterService instance at 0x7f11548843b0>
[hrpsys.py]   eval : [self.co, self.co_svc, self.co_version] = self.createComp("CollisionDetector","co")
[hrpsys.py] create Comp -> CollisionDetector : <hrpsys.rtm.RTcomponent instance at 0x7f115487bcb0> (315.15.0)
[hrpsys.py] create CompSvc -> CollisionDetector Service : <OpenHRP._objref_CollisionDetectorService instance at 0x7f1154884878>
[hrpsys.py]   eval : [self.sc, self.sc_svc, self.sc_version] = self.createComp("ServoController","sc")
[hrpsys.py] create Comp -> ServoController : <hrpsys.rtm.RTcomponent instance at 0x7f1154879d88> (315.15.0)
[hrpsys.py] create CompSvc -> ServoController Service : <OpenHRP._objref_ServoControllerService instance at 0x7f1154888560>
[hrpsys.py]   eval : [self.log, self.log_svc, self.log_version] = self.createComp("DataLogger","log")
[hrpsys.py] create Comp -> DataLogger : <hrpsys.rtm.RTcomponent instance at 0x7f115487cb00> (315.15.0)
[hrpsys.py] create CompSvc -> DataLogger Service : <OpenHRP._objref_DataLoggerService instance at 0x7f11548844d0>
[hrpsys.py]   eval : [self.rmfo, self.rmfo_svc, self.rmfo_version] = self.createComp("RemoveForceSensorLinkOffset","rmfo")
[hrpsys.py] create Comp -> RemoveForceSensorLinkOffset : <hrpsys.rtm.RTcomponent instance at 0x7f115487b998> (315.15.0)
[hrpsys.py] create CompSvc -> RemoveForceSensorLinkOffset Service : <OpenHRP._objref_RemoveForceSensorLinkOffsetService instance at 0x7f11548820e0>
[hrpsys.py] connecting hrpsys components
[rtm.py]    Connect sh.qOut - ic.qRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect ic.q - co.qRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect co.q - el.qRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect el.q - RobotHardware0.qRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.servoState - el.servoStateIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.q - sh.currentQIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.q - fk.q (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.qOut - fk.qRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.qRef - sh.qIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.tqRef - sh.tqIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.basePos - sh.basePosIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.baseRpy - sh.baseRpyIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.zmpRef - sh.zmpIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.optionalData - sh.optionalDataIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.basePosOut - seq.basePosInit (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.basePosOut - fk.basePosRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.baseRpyOut - seq.baseRpyInit (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.baseRpyOut - fk.baseRpyRef (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.qOut - seq.qInit (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.zmpOut - seq.zmpRefInit (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.rhsensorRef - sh.rhsensorIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect seq.lhsensorRef - sh.lhsensorIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.rhsensorOut - ic.ref_rhsensorIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.lhsensorOut - ic.ref_lhsensorIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.q - rmfo.qCurrent (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.rhsensor - rmfo.rhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect rmfo.off_rhsensor - ic.rhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.lhsensor - rmfo.lhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect rmfo.off_lhsensor - ic.lhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.q - ic.qCurrent (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.basePosOut - ic.basePosIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect sh.baseRpyOut - ic.baseRpyIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.q - co.qCurrent (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.servoState - co.servoStateIn (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[rtm.py]    Connect RobotHardware0.q - el.qCurrent (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py] activating hrpsys components
[hrpsys.py] setup hrpsys logger
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = q to RobotHardware0_q
[rtm.py]    Connect RobotHardware0.q - log.RobotHardware0_q (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = dq to RobotHardware0_dq
[rtm.py]    Connect RobotHardware0.dq - log.RobotHardware0_dq (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = tau to RobotHardware0_tau
[rtm.py]    Connect RobotHardware0.tau - log.RobotHardware0_tau (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py] sensor names for DataLogger
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = rhsensor to RobotHardware0_rhsensor
[rtm.py]    Connect RobotHardware0.rhsensor - log.RobotHardware0_rhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = lhsensor to RobotHardware0_lhsensor
[rtm.py]    Connect RobotHardware0.lhsensor - log.RobotHardware0_lhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = qOut to sh_qOut
[rtm.py]    Connect sh.qOut - log.sh_qOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = tqOut to sh_tqOut
[rtm.py]    Connect sh.tqOut - log.sh_tqOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedPoint3D, name = basePosOut to sh_basePosOut
[rtm.py]    Connect sh.basePosOut - log.sh_basePosOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedOrientation3D, name = baseRpyOut to sh_baseRpyOut
[rtm.py]    Connect sh.baseRpyOut - log.sh_baseRpyOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedPoint3D, name = zmpOut to sh_zmpOut
[rtm.py]    Connect sh.zmpOut - log.sh_zmpOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = q to ic_q
[rtm.py]    Connect ic.q - log.ic_q (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = q to el_q
[rtm.py]    Connect el.q - log.el_q (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : emergencySignal arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.emergencySignal - log.emergencySignal (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedLongSeqSeq, name = servoState to RobotHardware0_servoState
[rtm.py]    Connect RobotHardware0.servoState - log.RobotHardware0_servoState (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = rhsensorOut to sh_rhsensorOut
[rtm.py]    Connect sh.rhsensorOut - log.sh_rhsensorOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = lhsensorOut to sh_lhsensorOut
[rtm.py]    Connect sh.lhsensorOut - log.sh_lhsensorOut (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = off_rhsensor to rmfo_off_rhsensor
[rtm.py]    Connect rmfo.off_rhsensor - log.rmfo_off_rhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py]   setupLogger : record type = TimedDoubleSeq, name = off_lhsensor to rmfo_off_lhsensor
[rtm.py]    Connect rmfo.off_lhsensor - log.rmfo_off_lhsensor (dataflow_type=Push, subscription_type=flush, bufferlength=1, push_rate=1000, push_policy=new)
[hrpsys.py] setup joint groups for hrpsys controller
[hrpsys.py] initialized successfully
[nextage.py] No ROS Master found. Without it, you cannot use ROS from this script, but you can still use RTM without issues. To use ROS, do not forget to run rosbridge. How to do so? --> http://wiki.ros.org/rtmros_nextage/Tutorials/Operating%20Hiro%2C%20NEXTAGE%20OPEN

In [1]: robot.checkEncoders()
[hrpsys.py]  calib-joint all 
[hrpsys.py]  done
Turn on Hand Servo

In [2]: robot.HandClose('lhand')

In [3]: robot.HandOpen('rhand')

robot.HandClose('lhand') has no problem, but robot.HandOpen('rhand') unexpectedly moved left hand.
You can see the behavior in this video: https://drive.google.com/open?id=19yFCOOD3o0bqJKtdLQQGNjTPPqdKmtbf

Cause & Solution

setJointAnglesOfGroup has a bug.
The length of id, tms, and rad is len (4 in HIRONX), but servo_id.size() (8 in HIRONX) is passed as their length to setPositions.
After this PR, len is passed to setPositions.

Other Minor Fix

I found wrong pointer passing in sendPacket.
As echo is array, &echo which is passed to read means pointer of pointer.
read requires simple pointer, so simply passing echo is correct.