search

tork-a / rtmros_nextage

ROS-OpenRTM-based opensource robot controller software for dual-armed robot Nextage from Kawada Industries
http://wiki.ros.org/rtmros_nextage
27 stars 39 forks source link

How to make startImpedance working? #304

Open shzargar opened 7 years ago

shzargar commented 7 years ago

For Nextage open, hrpsys 315.2.8, in python client, startImpedance is not working.

on the laptop I have this:

robot.startImpedance('rarm')
---------------------------------------------------------------------------
TypeError                                 Traceback (most recent call last)
<ipython-input-1-263aa4e2f182> in <module>()
----> 1 robot.startImpedance('rarm')

/opt/ros/indigo/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.pyc in startImpedance(self, arm, **kwargs)
    960             self.startImpedance_315_1(arm, **kwargs)
    961         elif StrictVersion(self.hrpsys_version) < StrictVersion('315.3.0'):
--> 962             self.startImpedance_315_2(arm, **kwargs)
    963         else:
    964             self.startImpedance_315_3(arm, **kwargs)

/opt/ros/indigo/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.pyc in startImpedance_315_2(self, arm, M_p, D_p, K_p, M_r, D_r, K_r, force_gain, moment_gain, sr_gain, avoid_gain, reference_gain, manipulability_limit)
    915                 avoid_gain = avoid_gain,
    916                 reference_gain = reference_gain,
--> 917                 manipulability_limit = manipulability_limit))
    918         return self.ic_svc.startImpedanceController(arm)
    919 

TypeError: __init__() takes exactly 16 arguments (13 given)

On the nextage computer, I got:

robot.startImpedance('larm')
---------------------------------------------------------------------------
AttributeError                            Traceback (most recent call last)
/home/nxouser/<ipython-input-2-9c0fcf1fd550> in <module>()
----> 1 robot.startImpedance('larm')

/opt/ros/hydro/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.pyc in startImpedance(self, arm, **kwargs)
    796             self.startImpedance_315_1(arm, **kwargs)
    797         elif self.hrpsys_version < '315.3.0':
--> 798             self.startImpedance_315_2(arm, **kwargs)
    799         else:
    800             self.startImpedance_315_3(arm, **kwargs)

/opt/ros/hydro/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.pyc in startImpedance_315_2(self, arm, M_p, D_p, K_p, M_r, D_r, K_r, force_gain, moment_gain, sr_gain, avoid_gain, reference_gain, manipulability_limit)
    737                        reference_gain = 0.0,
    738                        manipulability_limit = 0.1):
--> 739         self.ic_svc.setImpedanceControllerParam(
    740             arm,
    741             OpenHRP.ImpedanceControllerService.impedanceParam(

AttributeError: 'NextageClient' object has no attribute 'ic_svc'

as well running ipython on the nextage computer results in (I cannot see [rtm.py] Connect sh.rhsensorOut - ic.ref_rhsensorIn and [rtm.py] Connect sh.lhsensorOut - ic.ref_lhsensorIn) :

ge.py -- --host nextage
Python 2.7.3 (default, Jun 22 2015, 19:33:41) 
Type "copyright", "credits" or "license" for more information.

IPython 0.12.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.
configuration ORB with nextage:15005
[hrpsys.py] wait for RTCmanager : None
[hrpsys.py] wait for RobotHardware0 : <hrpsys.rtm.RTcomponent instance at 0x46f7ea8> ( timeout 0 < 10)
[hrpsys.py] findComps -> RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x46f7ea8> isActive? = True 
[hrpsys.py] simulation_mode : False
[hrpsys.py]  Hrpsys controller version info: 
[hrpsys.py]    ms =  <hrpsys.rtm.RTCmanager instance at 0x4746200>
[hrpsys.py]    ref =  <RTM._objref_Manager instance at 0x46f7e60>
[hrpsys.py]    version  =  315.2.8
[hrpsys.py] waiting ModelLoader
[hrpsys.py] start hrpsys
[hrpsys.py] finding RTCManager and RobotHardware
[hrpsys.py] wait for RTCmanager : None
[hrpsys.py] wait for RobotHardware0 : <hrpsys.rtm.RTcomponent instance at 0x46d3b48> ( timeout 0 < 10)
[hrpsys.py] findComps -> RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x46d3b48> isActive? = True 
[hrpsys.py] simulation_mode : False
[hrpsys.py] creating components
[hrpsys.py]   eval : [self.seq, self.seq_svc, self.seq_version] = self.createComp("SequencePlayer","seq")
RTC named "seq" already exists.
[hrpsys.py] create Comp -> SequencePlayer : <hrpsys.rtm.RTcomponent instance at 0x474bc20> (315.2.8)
[hrpsys.py] create CompSvc -> SequencePlayer Service : <OpenHRP._objref_SequencePlayerService instance at 0x4745c20>
[hrpsys.py]   eval : [self.sh, self.sh_svc, self.sh_version] = self.createComp("StateHolder","sh")
RTC named "sh" already exists.
[hrpsys.py] create Comp -> StateHolder : <hrpsys.rtm.RTcomponent instance at 0x4755e60> (315.2.8)
[hrpsys.py] create CompSvc -> StateHolder Service : <OpenHRP._objref_StateHolderService instance at 0x497a128>
[hrpsys.py]   eval : [self.fk, self.fk_svc, self.fk_version] = self.createComp("ForwardKinematics","fk")
RTC named "fk" already exists.
[hrpsys.py] create Comp -> ForwardKinematics : <hrpsys.rtm.RTcomponent instance at 0x455e290> (315.2.8)
[hrpsys.py] create CompSvc -> ForwardKinematics Service : <OpenHRP._objref_ForwardKinematicsService instance at 0x4756e60>
[hrpsys.py]   eval : [self.el, self.el_svc, self.el_version] = self.createComp("SoftErrorLimiter","el")
RTC named "el" already exists.
[hrpsys.py] create Comp -> SoftErrorLimiter : <hrpsys.rtm.RTcomponent instance at 0x4620f38> (315.2.8)
[hrpsys.py] create CompSvc -> SoftErrorLimiter Service : <OpenHRP._objref_SoftErrorLimiterService instance at 0x497bfc8>
[hrpsys.py]   eval : [self.log, self.log_svc, self.log_version] = self.createComp("DataLogger","log")
RTC named "log" already exists.
[hrpsys.py] create Comp -> DataLogger : <hrpsys.rtm.RTcomponent instance at 0x49731b8> (315.2.8)
[hrpsys.py] create CompSvc -> DataLogger Service : <OpenHRP._objref_DataLoggerService instance at 0x4759518>
[hrpsys.py] connecting components
[rtm.py]    Connect sh.qOut - el.qRef
[rtm.py]    Connect el.q - RobotHardware0.qRef
[rtm.py]    Connect RobotHardware0.servoState - el.servoStateIn
[rtm.py]    Connect RobotHardware0.q - sh.currentQIn
[rtm.py]    Connect RobotHardware0.q - fk.q
[rtm.py]    Connect sh.qOut - fk.qRef
[rtm.py]    Connect seq.qRef - sh.qIn
[rtm.py]    Connect seq.tqRef - sh.tqIn
[rtm.py]    Connect seq.basePos - sh.basePosIn
[rtm.py]    Connect seq.baseRpy - sh.baseRpyIn
[rtm.py]    Connect seq.zmpRef - sh.zmpIn
[rtm.py]    Connect seq.optionalData - sh.optionalDataIn
[rtm.py]    Connect sh.basePosOut - seq.basePosInit
[rtm.py]    Connect sh.basePosOut - fk.basePosRef
[rtm.py]    Connect sh.baseRpyOut - seq.baseRpyInit
[rtm.py]    Connect sh.baseRpyOut - fk.baseRpyRef
[rtm.py]    Connect sh.qOut - seq.qInit
[rtm.py]    Connect sh.zmpOut - seq.zmpRefInit
[rtm.py]    Connect RobotHardware0.q - el.qCurrent
[hrpsys.py] activating components
seqis already serialized
shis already serialized
fkis already serialized
elis already serialized
logis already serialized
[hrpsys.py] setup logger
[hrpsys.py]   setupLogger : q arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.q - log.RobotHardware0_q
[hrpsys.py]   setupLogger : dq arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.dq - log.RobotHardware0_dq
[hrpsys.py]   setupLogger : tau arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.tau - log.RobotHardware0_tau
[hrpsys.py] sensor names for DataLogger
[hrpsys.py]   setupLogger : qOut arleady exists in DataLogger
[rtm.py]    Connect sh.qOut - log.sh_qOut
[hrpsys.py]   setupLogger : tqOut arleady exists in DataLogger
[rtm.py]    Connect sh.tqOut - log.sh_tqOut
[hrpsys.py]   setupLogger : basePosOut arleady exists in DataLogger
[rtm.py]    Connect sh.basePosOut - log.sh_basePosOut
[hrpsys.py]   setupLogger : baseRpyOut arleady exists in DataLogger
[rtm.py]    Connect sh.baseRpyOut - log.sh_baseRpyOut
[hrpsys.py]   setupLogger : zmpOut arleady exists in DataLogger
[rtm.py]    Connect sh.zmpOut - log.sh_zmpOut
[hrpsys.py]   setupLogger : emergencySignal arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.emergencySignal - log.emergencySignal
[hrpsys.py]   setupLogger : servoState arleady exists in DataLogger
[rtm.py]    Connect RobotHardware0.servoState - log.RobotHardware0_servoState
[hrpsys.py] setup joint groups
[hrpsys.py] initialized successfully
[INFO] [WallTime: 1490739828.644979] Joint names; Torso: ['CHEST_JOINT0']
    Head: ['HEAD_JOINT0', 'HEAD_JOINT1']
    L-Arm: ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5']
    R-Arm: ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
[ INFO] [1490739828.658671290]: Loading robot model 'NextageOpen'...
[ INFO] [1490739829.850569929]: Ready to take MoveGroup commands for group left_arm.
[ INFO] [1490739830.032259612]: Ready to take MoveGroup commands for group right_arm.
130s commented 7 years ago

Impedance control feature on the real robot requires a special service running on the controller (in QNX), and how to do so isn't disclosed. TORK provides a paid service for it. You also need F/T sensors too at your expense.

You will be able to test a the feature on simulator. Good news (maybe) is that I'm working on it right now #302