yaw movement with setTargetPoseRelative actually moves roll #335

130s · 2015-03-17T03:24:37Z

See the youtube video; Both roll movement that comes as the 1st batch, and yaw, 3rd, rotate about red axis (x) while we expect yaw rotates about blue axis.

Result of getCurrentRPY('RARM_JOINT5') indicates that yaw is rotated as expected in the 3rd batch of the movements.

$ ipython -i ~/Desktop/nextage_RPY_position_test.py 
:
[hrpsys.py]    setupLogger :  emergencySignal  arleady exists in DataLogger
HiroNX(Robot)0.emergencySignal and log.emergencySignal are already connected
[hrpsys.py]  setup joint groups
[hrpsys.py]  initialized successfully
[hrpsys.py] , JntAnglesOfGr=torso, INITPOSE[i]=[0], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=head, INITPOSE[i]=[0, 0], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=rarm, INITPOSE[i]=[-0.6, 0, -100, 15.2, 9.4, 3.2], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=larm, INITPOSE[i]=[0.6, 0, -100, -15.2, 9.4, -3.2], tm=3, wait=True
goInitial (0.1790002385019105, -1.5700280609383033, -0.17776428666024424) (-0.1790002385019105, -1.5700280609383033, 0.17776428666024424)
[ 0.32484857 -0.18201862  1.03680264] [ 1.74979657 -1.57002806 -0.17776429]
rarm None [0.32484856729528888, -0.18201862397166299, 1.0368026417623699] [1.749796565296807, -1.5700280609383033, -0.17776428666024424] 0.5
[ 0.32484857  0.18201862  1.03680264] [ 1.39179609 -1.57002806  0.17776429]
larm None [0.32484856729528888, 0.18201862397166299, 1.0368026417623699] [1.3917960882929861, -1.5700280609383033, 0.17776428666024424] 0.5
Roll +move  (0.1790002385019105, -1.5700280609383033, -0.17776428666024424) (-0.1790002385019105, -1.5700280609383033, 0.17776428666024424)
[ 0.32484839 -0.1820044   1.03680268] [ 0.15324298 -1.57002812 -0.17760291]
rarm None [0.32484838559554136, -0.18200440398211598, 1.0368026803470394] [0.15324298212371934, -1.5700281203401243, -0.17760290598143333] 0.5
[ 0.32484863  0.18203201  1.03680262] [-0.21436701 -1.57002801  0.17790798]
larm None [0.32484863037008305, 0.18203200857881313, 1.0368026224539324] [-0.21436700764251593, -1.5700280103287043, 0.17790797525715421] 0.5
Roll -move (1.744656077208585, -1.5700281208977755, -0.17760154578253698) (1.3660562556193259, -1.5700280101240005, 0.1779084821051138)
[ 0.3248485  -0.18201442  1.03680263] [ 1.74974638 -1.57002808 -0.17771547]
rarm None [0.32484849621018302, -0.18201442349228358, 1.0368026254281859] [1.7497463834078084, -1.5700280822997297, -0.17771546909653543] 0.5
[ 0.32484838  0.18202196  1.03680271] [ 1.3909327  -1.57002805  0.17778358]
larm None [0.32484838182331555, 0.18202196252587466, 1.0368027058128837] [1.3909326953942442, -1.5700280546272203, 0.17778358010850423] 0.5
Roll +move  (0.15361466516427447, -1.5700280823072363, -0.17771550127346952) (-0.2098782738823774, -1.570028054790546, 0.17778324845150964)
[ 0.32484831 -0.18200027  1.03680266] [ 0.1438193  -1.57002814 -0.17755467]
rarm None [0.3248483131389025, -0.18200027174288302, 1.0368026648250896] [0.14381929862865706, -1.5700281414490111, -0.1775546713947975] 0.5
[ 0.32484844  0.18203526  1.03680269] [-0.20534076 -1.570028    0.17792629]
larm None [0.32484844344799041, 0.18203526076210838, 1.0368026872556013] [-0.20534075613557379, -1.570028004328855, 0.17792629438896618] 0.5
Roll -move (1.7245984660662992, -1.570028105203389, -0.177660156817488) (1.3654555706593228, -1.570028004328855, 0.17792629438896618)
[ 0.32484842 -0.18201019  1.03680261] [ 1.74910064 -1.5700281  -0.17766615]
rarm None [0.32484842287336058, -0.18201019020085168, 1.0368026103862076] [1.7491006416851793, -1.5700281037703667, -0.17766615262010349] 0.5
[ 0.32484819  0.18202512  1.03680277] [ 1.39117159 -1.57002805  0.17780066]
larm None [0.32484819260901748, 0.18202511642228059, 1.0368027713795751] [1.3911715872613182, -1.570028049083094, 0.17780066464986891] 0.5
Roll +move  (0.14418737574345405, -1.5700281037840633, -0.1776662111497786) (-0.20052820181824185, -1.5700280491913066, 0.17780043424131944)
[ 0.32484824 -0.18199612  1.03680265] [ 0.15281004 -1.57002816 -0.17750644]
rarm None [0.32484824262258183, -0.18199611757921039, 1.0368026486901556] [0.15281004220255756, -1.570028162532829, -0.1775064419389521] 0.5
[ 0.32484825  0.18203855  1.03680275] [-0.20536445 -1.570028    0.1779448 ]
larm None [0.32484825469905326, 0.18203855159740079, 1.036802752661967] [-0.20536444984092772, -1.5700279982759591, 0.17794480488449974] 0.5
Roll -move (1.7486814350099888, -1.5700281632060062, -0.17750479892282378) (1.3907658049943628, -1.570027997720863, 0.177946187132938)
[ 0.32484835 -0.18200614  1.03680259] [ 1.7493093  -1.57002812 -0.17761901]
rarm None [0.32484835349148611, -0.18200613574410668, 1.0368025937198257] [1.7493093016035381, -1.570028124508033, -0.17761901073279637] 0.5
[ 0.324848    0.18202842  1.03680284] [ 1.40052605 -1.57002804  0.17781926]
larm None [0.32484800372723144, 0.1820284178900532, 1.0368028369100606] [1.4005260532154342, -1.5700280429911573, 0.17781926340213383] 0.5
Roll +move  (0.1531816882957567, -1.5700281245153878, -0.17761904224871813) (-0.20055112205743592, -1.570028043147831, 0.17781893043266497)
[ 0.32484817 -0.18199199  1.03680263] [ 0.1433823  -1.57002818 -0.1774582 ]
rarm None [0.32484816998453381, -0.18199198633981861, 1.0368026332326989] [0.14338229718932127, -1.5700281836348902, -0.17745820261119491] 0.5
[ 0.32484806  0.18204178  1.03680282] [-0.19575723 -1.57002799  0.17796252]
larm None [0.32484806433244667, 0.18204177573518329, 1.0368028189798939] [-0.19575722686942232, -1.5700279924647136, 0.17796251977462324] 0.5
Roll -move (1.748885705847304, -1.5700281845670165, -0.17745592746427152) (1.3960171852178769, -1.5700279920018114, 0.17796367456448128)
[ 0.32484828 -0.18200191  1.03680258] [ 1.73963271 -1.57002815 -0.17756972]
rarm None [0.32484827996384458, -0.18200190647282274, 1.0368025787498745] [1.7396327067945327, -1.5700281459691199, -0.17756972174664676] 0.5
[ 0.32484781  0.18203164  1.0368029 ] [ 1.40075514 -1.57002804  0.17783701]
larm None [0.32484781350733644, 0.18203164324825119, 1.0368029031909454] [1.4007551442652795, -1.5700280371805302, 0.17783700551888248] 0.5
Roll +move  (0.1437503966422288, -1.570028145979322, -0.177569764698699) (-0.1953723585159133, -1.5700280373120055, 0.1778367265153489)
[ 0.3248481  -0.18198783  1.03680262] [ 0.13371171 -1.5700282  -0.17740952]
rarm None [0.32484809511886348, -0.18198782733150745, 1.0368026190667539] [0.13371170595743509, -1.5700282048348182, -0.17740952058854015] 0.5
[ 0.32484787  0.18204526  1.03680289] [-0.17487557 -1.57002799  0.1779828 ]
larm None [0.32484787277885863, 0.18204525919403394, 1.0368028864405689] [-0.17487557442056945, -1.5700279856950057, 0.17798280224114096] 0.5
Roll -move (1.7392098331081038, -1.570028205745979, -0.17740729613445955) (1.3959207523743271, -1.5700279856950057, 0.17798280224114096)

[hrpsys.py] , JntAnglesOfGr=torso, INITPOSE[i]=[0], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=head, INITPOSE[i]=[0, 0], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=rarm, INITPOSE[i]=[-0.6, 0, -100, 15.2, 9.4, 3.2], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=larm, INITPOSE[i]=[0.6, 0, -100, -15.2, 9.4, -3.2], tm=3, wait=True
goInitial (0.17900023850191052, -1.5700280609383033, -0.17776428666024424) (-0.1790002385019105, -1.5700280609383033, 0.17776428666024424)
[ 0.32484857 -0.18201862  1.03680264] [ 0.17900024 -0.7846299  -0.17776429]
rarm None [0.32484856729528888, -0.18201862397166299, 1.0368026417623699] [0.17900023850191052, -0.78462989754085499, -0.17776428666024424] 0.5
[ 0.32484857  0.18201862  1.03680264] [-0.17900024 -0.7846299   0.17776429]
larm None [0.32484856729528888, 0.18201862397166299, 1.0368026417623699] [-0.17900023850191049, -0.78462989754085499, 0.17776428666024424] 0.5
Pitch +move (0.17900023850191052, -1.5700280609383033, -0.17776428666024424) (-0.1790002385019105, -1.5700280609383033, 0.17776428666024424)
[ 0.32483751 -0.18201589  1.03673047] [ 0.17899482 -1.58687499 -0.17774668]
rarm None [0.32483750690558671, -0.18201588722086362, 1.0367304687522561] [0.17899482230130678, -1.5868749935356916, -0.17774668420517789] 0.5
[ 0.32483751  0.18201589  1.03673047] [-0.17899482 -1.58687499  0.17774668]
larm None [0.32483750690558671, 0.18201588722086365, 1.0367304687522561] [-0.17899482230130681, -1.5868749935356916, 0.17774668420517792] 0.5
Pitch -move (0.17899214022030996, -0.7856243078093015, -0.17773978590281678) (-0.17899523061324066, -0.7903002471551012, 0.17774819651913826)
[ 0.32483778 -0.18201627  1.03674176] [-2.96156358 -0.78477515  2.96281066]
rarm None [0.32483778385274242, -0.18201626606402335, 1.0367417637937062] [-2.9615635796949618, -0.78477515246167995, 2.9628106575799236] 0.5
[ 0.32483799  0.1820163   1.03674165] [ 2.96254286 -0.77354334 -2.96378932]
larm None [0.32483799330144714, 0.18201630084941989, 1.036741653541118] [2.9625428639064562, -0.77354334144218262, -2.9637893203132184] 0.5
Pitch +move (-2.9626089968141813, -1.554842346839856, 2.9638587463747568) (2.9626088700293627, -1.554851456927956, -2.9638586112591323)
[ 0.3248266  -0.18201325  1.03678431] [-2.96156126 -1.58677843  2.96280575]
rarm None [0.32482659943787495, -0.18201325258876266, 1.0367843050930645] [-2.9615612589103426, -1.5867784312891184, 2.962805749419025] 0.5
[ 0.32482734  0.18201372  1.03678555] [ 2.96253836 -1.56376994 -2.96377934]
larm None [0.32482733733426938, 0.18201372099553931, 1.0367855548043796] [2.9625383617127503, -1.5637699447941209, -2.9637793406783426] 0.5
Pitch -move (-2.9615695698722098, -0.8088432159701218, 2.9628253049021622) (2.962545536460324, -0.7737280780338689, -2.9637971349099774)
[ 0.32482696 -0.18201306  1.03677154] [ 0.18007319 -0.76954161 -0.17882713]
rarm None [0.32482696186997334, -0.18201305575530252, 1.0367715422841461] [0.18007319270389938, -0.76954160991773901, -0.17882712891294786] 0.5
[ 0.32482786  0.1820136   1.03677228] [ 2.96263909 -0.77824134 -2.96388175]
larm None [0.32482786497560068, 0.18201360009279666, 1.0367722797442238] [2.9626390850200757, -0.77824133897542169, -2.963881751637262] 0.5
Pitch +move (0.18007319270389938, -1.5549397733151873, -0.17882712891294786) (2.9626390850200757, -1.56363950237287, -2.963881751637262)
[ 0.32481628 -0.18201087  1.03669861] [ 0.18006757 -1.5717707  -0.17880893]
rarm None [0.32481628228939013, -0.1820108745927605, 1.0366986075656721] [0.18006757005493776, -1.5717707004667738, -0.17880893298598355] 0.5
[ 0.3248169   0.18201073  1.03681462] [ 2.96263596 -1.57965939 -2.96387542]
larm None [0.32481690226409465, 0.18201072968477131, 1.0368146183315872] [2.9626359647568852, -1.5796593908004888, -2.9638754155476827] 0.5
Pitch -move (0.1800648972074896, -0.7705203550747363, -0.1788019575838592) (2.9626417754787475, -0.7784276636719153, -2.963889587854647)
[ 0.32481671 -0.18201133  1.03670973] [ 0.18002368 -0.77091727 -0.17876611]
rarm None [0.32481671058632022, -0.18201133353933796, 1.0367097347004324] [0.1800236767759103, -0.77091727128040222, -0.17876610579693838] 0.5
[ 0.32481886  0.18201109  1.03680101] [ 2.96269894 -0.77880442 -2.96393761]
larm None [0.3248188569138169, 0.18201109452609837, 1.0368010135328114] [2.9626989404156783, -0.7788044189118184, -2.9639376090292959] 0.5
Pitch +move (-2.961721403607041, -1.5699553751129214, 2.9629816403070177) (-0.17897487578314517, -1.5661580729846238, 0.1777370031793881)
[ 0.32480676 -0.18200958  1.03663846] [ 0.18001862 -1.56131206 -0.17874993]
rarm None [0.32480675881197701, -0.18200957939661036, 1.0366384578881698] [0.18001861954294604, -1.5613120605353152, -0.17874992809605353] 0.5
[ 0.32480834  0.18200846  1.0368437 ] [ 2.96269512 -1.56843288 -2.9639293 ]
larm None [0.3248083356220442, 0.18200846331826984, 1.0368436997474189] [2.9626951206376937, -1.5684328770500029, -2.9639293014001131] 0.5
Pitch -move (0.18001578094564571, -0.7718228146276673, -0.17874215065485374) (2.9626951206376937, -0.7830347136525546, -2.963929301400113)
[ 0.32480717 -0.18201006  1.03664992] [ 0.17999485 -0.75982034 -0.17872701]
rarm None [0.32480716882714078, -0.18201005664174064, 1.0366499217546024] [0.17999484662478171, -0.75982033985366582, -0.17872700649057954] 0.5
[ 0.32481094  0.18200904  1.03682944] [ 2.96270083 -0.7781626  -2.96393314]
larm None [0.32481094446699721, 0.18200904429492423, 1.0368294383651997] [2.9627008313896699, -0.77816260258144765, -2.9639331397865338] 0.5
Pitch +move (0.1800369634155156, -1.56118358649777, -0.1787665754161586) (2.962972725575836, -1.568255060789684, -2.9642085390031188)
[ 0.32479701 -0.18200829  1.03657892] [ 0.17998969 -1.56145398 -0.17871009]
rarm None [0.32479701120723031, -0.18200828659545251, 1.036578922169052] [0.17998969494800321, -1.5614539784430388, -0.17871008808157357] 0.5
[ 0.32479772  0.18200547  1.03687393] [ 2.96270367 -1.5637014  -2.96394136]
larm None [0.32479772313805, 0.18200546503207532, 1.0368739322509424] [2.9627036698102902, -1.56370139756122, -2.9639413606689842] 0.5
Pitch -move (0.1799868696172728, -0.7607156783713874, -0.17870271804117832) (2.96270366981029, -0.7783032341637716, -2.963941360668984)

[hrpsys.py] , JntAnglesOfGr=torso, INITPOSE[i]=[0], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=head, INITPOSE[i]=[0, 0], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=rarm, INITPOSE[i]=[-0.6, 0, -100, 15.2, 9.4, 3.2], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=larm, INITPOSE[i]=[0.6, 0, -100, -15.2, 9.4, -3.2], tm=3, wait=True

goInitial (0.1790002385019105, -1.5700280609383033, -0.17776428666024424) (-0.17900023850191446, -1.5700280609383033, 0.17776428666024174)
[ 0.32484857 -0.18201862  1.03680264] [ 0.17900024 -1.57002806  1.39303204]
rarm None [0.32484856729528888, -0.18201862397166299, 1.0368026417623699] [0.17900023850191049, -1.5700280609383033, 1.3930320401346523] 0.5
[ 0.32484857  0.18201862  1.03680264] [-0.17900024 -1.57002806  1.74856061]
larm None [0.32484856729528888, 0.18201862397166299, 1.0368026417623699] [-0.17900023850191446, -1.5700280609383033, 1.7485606134551384] 0.5
Yaw +move (0.17892332287603052, -1.5700279232500491, 1.388131208611012) (-0.17893573238419963, -1.5700279224689515, 1.7229004702986859)
[ 0.32484839 -0.18200425  1.03680269] [ 0.17892332 -1.57002792 -0.18266512]
rarm None [0.32484838507877456, -0.1820042479857695, 1.0368026853931331] [0.17892332287603052, -1.5700279232500491, -0.18266511818388453] 0.5
[ 0.32484862  0.18203221  1.03680263] [-0.17893126 -1.57002792  0.17743505]
larm None [0.32484862429903805, 0.18203221045588966, 1.0368026311462974] [-0.17893126373699234, -1.570027922575411, 0.17743505040444485] 0.5
Yaw -move (0.17903579830657773, -1.5700278666592287, -0.13250554922092506) (-0.1788083918576079, -1.5700279474209018, 0.20290670192185767)
[ 0.32484849 -0.18201414  1.03680264] [ 0.17903487 -1.57002786  1.41328356]
rarm None [0.32484849285650141, -0.18201413823631099, 1.0368026354039599] [0.17903486534859422, -1.570027864612483, 1.4132835587851225] 0.5
[ 0.32484838  0.18202207  1.03680271] [-0.17880658 -1.57002795  1.7483658 ]
larm None [0.32484837540117145, 0.18202206537963211, 1.0368027112384761] [-0.17880657787164969, -1.570027945052124, 1.7483658003019327] 0.5
Yaw +move (0.17895611285971538, -1.5700277012736459, 1.3783994722463186) (-0.17874211001725449, -1.5700278065756954, 1.7227056954404756)
[ 0.32484831 -0.18199984  1.03680268] [ 0.17895611 -1.5700277  -0.19239685]
rarm None [0.32484830921047181, -0.18199983952052215, 1.036802679930608] [0.17895611285971538, -1.5700277012736459, -0.19239685454857791] 0.5
[ 0.32484843  0.18203565  1.0368027 ] [-0.17873764 -1.57002781  0.17724028]
larm None [0.32484843225828436, 0.18203565168880206, 1.0368027006526603] [-0.17873764222658456, -1.5700278066815747, 0.17724027640421602] 0.5
Yaw -move (0.17907008747856357, -1.5700276320918671, -0.13305153021874855) (-0.17861478232400135, -1.5700278315048426, 0.20271193989617936)
[ 0.32484842 -0.18200976  1.03680263] [ 0.17906793 -1.57002764  1.40363011]
rarm None [0.32484841749185378, -0.18200975690487259, 1.0368026298178137] [0.17906793124961554, -1.5700276417761247, 1.4036301064490404] 0.5
[ 0.32484818  0.18202551  1.03680278] [-0.17861297 -1.57002783  1.74817104]
larm None [0.32484818347529532, 0.18202550667723333, 1.0368027807222835] [-0.17861296921462214, -1.5700278291358927, 1.748171039152878] 0.5
Yaw +move (0.1789879810401171, -1.5700274795111677, 1.3687455374380637) (-0.17854853960589095, -1.570027690652393, 1.722510972568766)
[ 0.32484823 -0.18199546  1.03680267] [ 0.17898943 -1.57002749 -0.19267962]
rarm None [0.32484823360015197, -0.18199545903261327, 1.0368026744179377] [0.17898943051683749, -1.5700274918351464, -0.19267962013289663] 0.5
[ 0.32484824  0.18203909  1.03680277] [-0.17854407 -1.57002769  0.17704555]
larm None [0.32484824018578101, 0.18203909281135572, 1.0368027701667224] [-0.17854407267212249, -1.5700276907576924, 0.17704555439076874] 0.5
Yaw -move (0.17910326938523718, -1.5700274335930853, -0.14186365046819835) (-0.17842122475550254, -1.5700277155587414, 0.20251722986619908)
[ 0.32484834 -0.18200548  1.03680262] [ 0.17910279 -1.57002742  1.41297187]
rarm None [0.32484834309460053, -0.18200547520695129, 1.0368026237788712] [0.1791027947662758, -1.5700274187706322, 1.4129718736781123] 0.5
[ 0.32484799  0.18202895  1.03680285] [-0.17842013 -1.57002773  1.75269429]
larm None [0.32484799143803234, 0.18202895349133452, 1.0368028502207201] [-0.17842012690564285, -1.5700277270144409, 1.7526942875609439] 0.5
Yaw +move (0.17902459115930197, -1.5700272578082302, 1.3786751652778342) (-0.17835544461354266, -1.5700275904157612, 1.7273645928516816)
[ 0.32484816 -0.18199124  1.03680267] [ 0.17902608 -1.57002727 -0.1830917 ]
rarm None [0.32484816202474215, -0.18199124320324819, 1.0368026671459034] [0.17902608031925685, -1.5700272695639259, -0.18309170321726098] 0.5
[ 0.32484805  0.18204239  1.03680284] [-0.17835131 -1.57002759  0.18157223]
larm None [0.32484804691772412, 0.18204239248395401, 1.0368028400886777] [-0.17835130955777373, -1.5700275906932897, 0.18157223458642724] 0.5
Yaw -move (0.1791403111213298, -1.5700272121891523, -0.13228395695663228) (-0.17822881492317497, -1.570027615910028, 0.20704426146681842)
[ 0.32484827 -0.18200126  1.03680262] [ 0.17913985 -1.5700272   1.422554  ]
rarm None [0.32484827166446212, -0.18200125794275676, 1.0368026163827726] [0.17913985476243882, -1.5700271973639506, 1.4225540032969155] 0.5
[ 0.3248478   0.18203226  1.03680292] [-0.17822771 -1.57002763  1.75722131]
larm None [0.32484779818646037, 0.18203225924767658, 1.0368029201371487] [-0.17822771014082436, -1.5700276273721634, 1.7572213121196703] 0.5
Yaw +move (0.17906287950562016, -1.5700270352856993, 1.3882561569675789) (-0.1781622454353612, -1.5700274907850362, 1.7318908261045707)
[ 0.32484809 -0.18198695  1.03680266] [ 0.17906438 -1.57002705 -0.17351075]
rarm None [0.32484809162966871, -0.18198695190198944, 1.0368026589346715] [0.17906437926332835, -1.5700270470397519, -0.17351075266489935] 0.5
[ 0.32484785  0.1820457   1.03680291] [-0.17815847 -1.5700275   0.18172771]
larm None [0.32484785349567491, 0.18204570039099835, 1.036802910052657] [-0.17815847331506568, -1.5700275035122275, 0.18172770639884139] 0.5
Yaw -move (0.1791765026454501, -1.5700269993290112, -0.16893618518582618) (-0.17803784378121837, -1.5700275284962872, 0.20686542861278825)

[hrpsys.py] , JntAnglesOfGr=torso, INITPOSE[i]=[0], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=head, INITPOSE[i]=[0, 0], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=rarm, INITPOSE[i]=[-0.6, 0, -100, 15.2, 9.4, 3.2], tm=3, wait=True
[hrpsys.py] , JntAnglesOfGr=larm, INITPOSE[i]=[0.6, 0, -100, -15.2, 9.4, -3.2], tm=3, wait=True

goInitial (0.1790002385019105, -1.5700280609383033, -0.17776428666024424) (-0.1790002385019105, -1.5700280609383033, 0.17776428666024424)

Code is below. I don't know yet how to run assertion check so I haven't added this as a unittest.

robot.goInitial(3)
print "goInitial", robot.getCurrentRPY('RARM_JOINT5'), robot.getCurrentRPY('LARM_JOINT5')

#---------------------roll motion--------------------------------------------------------------------
for i in range(0, 5):
    robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dr=math.pi/2, tm=0.5, wait=False)
    robot.setTargetPoseRelative('larm', 'LARM_JOINT5', dr=math.pi/2, tm=0.5)
    print "Roll +move ", robot.getCurrentRPY('RARM_JOINT5'), robot.getCurrentRPY('LARM_JOINT5')
    robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dr=-math.pi/2, dw=0, tm=0.5, wait=False)
    robot.setTargetPoseRelative('larm', 'LARM_JOINT5', dr=-math.pi/2, dw=0, tm=0.5)
    print "Roll -move", robot.getCurrentRPY('RARM_JOINT5'), robot.getCurrentRPY('LARM_JOINT5')
robot.goInitial(3)
print "goInitial", robot.getCurrentRPY('RARM_JOINT5'), robot.getCurrentRPY('LARM_JOINT5')

#---------------------pitch motion-------------------------------------------------------------------
for j in range(0, 5):
    robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dp=math.pi/4, tm=0.5, wait=False)
    robot.setTargetPoseRelative('larm', 'LARM_JOINT5', dp=math.pi/4, tm=0.5)
    print "Pitch +move", robot.getCurrentRPY('RARM_JOINT5'), robot.getCurrentRPY('LARM_JOINT5')
    robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dp=-math.pi/4, tm=0.5, wait=False)
    robot.setTargetPoseRelative('larm', 'LARM_JOINT5', dp=-math.pi/4, tm=0.5)
    print "Pitch -move", robot.getCurrentRPY('RARM_JOINT5'), robot.getCurrentRPY('LARM_JOINT5')
robot.goInitial(3)
print "goInitial", robot.getCurrentRPY('RARM_JOINT5'), robot.getCurrentRPY('LARM_JOINT5')

#---------------------yaw motion---------------------------------------------------------------------
for j in range(0, 5):
    robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dw=math.pi/2, tm=0.5, wait=False)
    robot.setTargetPoseRelative('larm', 'LARM_JOINT5', dw=math.pi/2, tm=0.5)
    print "Yaw +move", robot.getCurrentRPY('RARM_JOINT5'), robot.getCurrentRPY('LARM_JOINT5')
    robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dw=-math.pi/2, tm=0.5, wait=False)
    robot.setTargetPoseRelative('larm', 'LARM_JOINT5', dw=-math.pi/2, tm=0.5)
    print "Yaw -move", robot.getCurrentRPY('RARM_JOINT5'), robot.getCurrentRPY('LARM_JOINT5')
robot.goInitial(3)
print "goInitial", robot.getCurrentRPY('RARM_JOINT5'), robot.getCurrentRPY('LARM_JOINT5')

The text was updated successfully, but these errors were encountered:

130s · 2015-03-17T03:24:56Z

#BTW, the video also shows that with pitch movement the y axis (green) is moving while it's not supposed to. This maybe related to #287

130s · 2015-03-17T03:29:00Z

Used the following. I had to apply this patch for hrpsys.

$ dpkg -p ros-indigo-openhrp3 ros-indigo-hrpsys ros-indigo-rtmros-common ros-indigo-rtmros-hironx ros-indigo-rtmros-nextage|grep Vers
Version: 3.1.7-15trusty-20150213-1223-+0000
Version: 315.3.0-1trusty-20150311-1141-+0000
Version: 1.2.8-0trusty-20150311-1229-+0000
Version: 1.0.28-0trusty-20150311-1244-+0000
Version: 0.6.0-0trusty-20150311-1247-+0000

k-okada · 2015-03-17T05:30:45Z

it seems relative means relative respect to RARM_LINK5 local coordinates and the convention is

red = x = roll
green = y = pitch
blue = z = yaw

so this seems ok.
if you wan to move RARM_LINK5, relative to world coordinates, may be you can add a few line to hrpsys_config.py,

also please look at https://github.com/start-jsk/rtmros_hironx/blob/hydro-devel/hironx_ros_bridge/test/test_hironx_cartesian.py#L88 to learn how to write test code.

130s · 2015-03-23T02:34:13Z

it seems relative means relative respect to RARM_LINK5 local coordinates and the convention is
red = x = roll
green = y = pitch
blue = z = yaw
so this seems ok.

Thank you for the explanation.

What about blue = z = yaw? In the video yaw movement didn't happen while it was supposed to. Instead roll moved again.

emijah · 2015-03-23T04:59:02Z

This is because of how the RPY gets calculated. When the elbow is rotated 90 degrees the rotation itself can become ambiguous.

This is the reason I was trying to push the setTargetPoseMatrix interface. We've implemented this using python and are happy with this for now.
I'd like to push this patch within the next few days.

k-okada · 2015-03-23T17:13:37Z

I see,

something like

tds.data[11]])
            matRef = numpy.array([tds.data[0:3], tds.data[4:7], 
tds.data[8:11]])
            posRef += [dx, dy, dz]
            matRef = matRef.dot(numpy.array(euler_matrix(dr,dp,dw)[:3,:3]))
            rpyRef = euler_from_matrix(matRef)
            print posRef, rpyRef
            ret = self.setTargetPose(gname, list(posRef), list(rpyRef), tm)

may solve the problem

--
◉ Kei Okada

On Mon, Mar 23, 2015 at 1:59 PM, Hajime SAITO [email protected]
wrote:

This is because of how the RPY gets calculated. When the elbow is rotated
90 degrees the rotation itself can become ambiguous.

This is the reason I was trying to push the setTargetPoseMatrix interface.
We've implemented this using python and are happy with this for now.

—
Reply to this email directly or view it on GitHub
#335 (comment)
.

130s · 2015-04-01T01:31:31Z

Addressed in the upstream fkanehiro/hrpsys-base#543.

Add a testcase for checking #335 (2nd trial)

130s added the critical label Mar 17, 2015

130s pushed a commit to 130s/rtmros_hironx that referenced this issue Mar 28, 2015

Add a testcase for checking start-jsk#335 (w/o yaw yet)

c2f37d9

130s pushed a commit to 130s/rtmros_hironx that referenced this issue Mar 28, 2015

Add a testcase for checking start-jsk#335 (w/o yaw yet)

2812f7a

130s mentioned this issue Mar 29, 2015

(hrpsys python) Correct relative rotation. fkanehiro/hrpsys-base#543

Merged

130s closed this as completed Apr 1, 2015

130s self-assigned this Apr 1, 2015

130s pushed a commit to 130s/rtmros_hironx that referenced this issue Aug 14, 2015

Add a testcase for checking start-jsk#335

54ee499

130s pushed a commit to 130s/rtmros_hironx that referenced this issue Aug 21, 2015

Add a testcase for checking start-jsk#335

e883d72

130s pushed a commit to 130s/rtmros_hironx that referenced this issue Aug 23, 2015

Add a testcase for checking start-jsk#335

4756556

130s pushed a commit to 130s/rtmros_hironx that referenced this issue Nov 9, 2015

Add a testcase for checking start-jsk#335

d942819

130s pushed a commit to 130s/rtmros_hironx that referenced this issue Nov 11, 2015

Add a testcase for checking start-jsk#335

2f9a3f4

130s pushed a commit to 130s/rtmros_hironx that referenced this issue Nov 11, 2015

Add a testcase for checking start-jsk#335

18069dd

130s added a commit that referenced this issue Nov 11, 2015

Merge pull request #407 from 130s/fix/relative_rpy_2

131b2b7

Add a testcase for checking #335 (2nd trial)

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

yaw movement with setTargetPoseRelative actually moves roll #335

yaw movement with setTargetPoseRelative actually moves roll #335

130s commented Mar 17, 2015

130s commented Mar 17, 2015

130s commented Mar 17, 2015

k-okada commented Mar 17, 2015

130s commented Mar 23, 2015

emijah commented Mar 23, 2015

k-okada commented Mar 23, 2015

130s commented Apr 1, 2015

yaw movement with setTargetPoseRelative actually moves roll #335

yaw movement with setTargetPoseRelative actually moves roll #335

Comments

130s commented Mar 17, 2015

130s commented Mar 17, 2015

130s commented Mar 17, 2015

k-okada commented Mar 17, 2015

130s commented Mar 23, 2015

emijah commented Mar 23, 2015

k-okada commented Mar 23, 2015

130s commented Apr 1, 2015