Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Programming Hiro / NEXTAGE OPEN (RTM)
Description: More OpenRTM-based commands of Hiro (Open) / NEXTAGE OPEN are introduced in scenario-oriented manner. Then how to write your own python script is explained.Tutorial Level: INTERMEDIATE
Next Tutorial: Programming Hiro/NEXTAGE OPEN in ROS
Contents
NOTE: All the tutorials available under the URL http://wiki.ros.org/rtmros_nextage/Tutorials are applicable to the multiple products of Kawada Industries; Hiro (only with the one that opensource software is installed) and NEXTAGE OPEN. To simplify the notation in the rest of the tutorials, we use HiroNXO to appoint the aforementioned robots.
Intro
In one of the previous tutorials we've already gone through operating the robot via python script on ipython terminal. In this tutorial, first we work on more operation based on a sample scenario, which lets you get more sense of the HiroNX python API. Then we look into a python sample code that does operation via HixoNXO python API.
To launch interactive session, try: NEXTAGE:
ipython -i `rospack find nextage_ros_bridge`/script/nextage.py (Simulation) ipython -i `rospack find nextage_ros_bridge`/script/nextage.py -- --host nextage (Real robot example)
Hironx:
ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py (Simulation) ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py -- --host hiro014 (Real robot example. Change `host` value accordingly to your robot)
or
hrpsyspy # In this case, the instantiated variable is hcf, not robot
Operation via interactive mode
More in-detail of interactive operation via python I/F
During initialization, robot's client interface class, HIRONX / NextageClient, is instantiated class as a variable robot on ipython terminal. Now see what's available inside of robot by:
In : robot.
You can tab-complete to know what's available:
robot.Groups robot.getCurrentRPY robot.rh_svc robot.HandClose robot.getCurrentRotation robot.saveLog robot.HandGroups robot.getJointAngles robot.sc robot.HandOpen robot.getRTCInstanceList robot.sc_svc robot.InitialPose robot.getRTCList robot.sensors robot.OffPose robot.getReferencePose robot.seq robot.RtcList robot.getReferencePosition robot.seq_svc robot.abc robot.getReferenceRPY robot.servoOff robot.activateComps robot.getReferenceRotation robot.servoOn robot.afs robot.getSensors robot.setHandEffort robot.checkEncoders robot.goActual robot.setHandJointAngles robot.clearLog robot.goInitial robot.setHandWidth robot.co robot.goOffPose robot.setJointAngle robot.co_svc robot.hand_width2angles robot.setJointAngles robot.configurator_name robot.hgc robot.setJointAnglesOfGroup robot.connectComps robot.ic robot.setSelfGroups robot.connectLoggerPort robot.init robot.setTargetPose robot.createComp robot.isCalibDone robot.setupLogger robot.createComps robot.isServoOn robot.sh robot.el robot.kf robot.sh_svc robot.el_svc robot.lengthDigitalInput robot.simulation_mode robot.ep_svc robot.lengthDigitalOutput robot.st robot.findModelLoader robot.liftRobotUp robot.stOff robot.fk robot.loadPattern robot.tf robot.fk_svc robot.log robot.vs robot.flat2Groups robot.log_svc robot.waitForModelLoader robot.getActualState robot.moveHand robot.waitForRTCManagerAndRoboHardware robot.getBodyInfo robot.ms robot.waitInterpolation robot.getCurrentPose robot.readDigitalInput robot.waitInterpolationOfGroup robot.getCurrentPosition robot.rh robot.writeDigitalOutput
See the list of joint groups and its member links:
In : robot.Groups Out: [['torso', ['CHEST_JOINT0']], ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']], ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']], ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5']]]
An advantage of ipython as a script interpreter is that you can get API info on itself. Suppose you're looking for how to get the current pose but you have no idea of command, first just guess and tab-complete:
In : robot.getCurrent robot.getCurrentPose robot.getCurrentPosition robot.getCurrentRPY robot.getCurrentRotation
Here you see four options, and clearly getCurrentPose sounds like it. You still need to know the arguments of the method. So put "?" at the end of the command:
In : robot.getCurrentPose? Type: instancemethod Base Class: <type 'instancemethod'> String Form:<bound method HIRONX.getCurrentPose of <__main__.HIRONX instance at 0x1f39758>> Namespace: Interactive File: /opt/ros/hydro/lib/python2.7/dist-packages/hrpsys_config.py Definition: robot.getCurrentPose(self, lname) Docstring: <no docstring>
Here you know getCurrentPose takes lname (stands for link name). So run:
In: robot.getCurrentPose('RARM_JOINT0') Out: [0.912826202314136, -0.4083482880688395, 0.0, 0.0, 0.39443415756662026, 0.8817224037285941, -0.25881904510252074, -0.145, 0.1056883139872261, 0.2362568060275051, 0.9659258262890683, 0.370296, 0.0, 0.0, 0.0, 1.0]
This getCurrentPose returns the rotational matrix and the position of the given link in 1 dimensional list. You could also get position only by another method:
In: robot.getCurrent robot.getCurrentPose robot.getCurrentPosition robot.getCurrentRPY robot.getCurrentRotation In : robot.getCurrentPosition('RARM_JOINT0') Out: [0.0, -0.145, 0.370296]
In hrpsys, position vector is presented as a list with three elements [ x, y, z ] where:
x: forward y: left z: upward
Now let's move an arm. First go to initial pose.
In : robot.goInitial()
In : robot.setTargetPose? Type: instancemethod Base Class: <type 'instancemethod'> String Form:<bound method HIRONX.setTargetPose of <__main__.HIRONX instance at 0x333b758>> Namespace: Interactive File: /opt/ros/hydro/lib/python2.7/dist-packages/hrpsys/hrpsys_config.py Definition: robot.setTargetPose(self, gname, pos, rpy, tm) Docstring: <no docstring>
gname is the name of the joint group. pos and rpy are in list format. First, store the current values into variables:
In : pos = robot.getCurrentPosition('RARM_JOINT5') In : rpy = robot.getReferenceRPY('RARM_JOINT5') In : tm = 3
Current pose should look like:
Then randomly manipulate position, and execute:
In : pos[2] = 0.1 In : robot.setTargetPose('rarm', pos, rpy, tm) Out: True
Now a plan is successfully shown for the right arm to be moved to the specified pose like this image.
NOTE that this image was taken while moveit was active. Ignore that the arm at the starting is displayed in light green, which id because of MoveIt!, and has nothing to do with this tutorial):
After awhile, when you're done with the robot, store both arms back to the power off poses by:
In : robot.goOffPose()
DIO operation (NEXTAGE OPEN only)
Please see rtmros_nextage/Tutorials/Using digital IO (NXO only).
Location of source and document of hrpsys-based API
API document is available separately in the following locations:
Due to the limitation that comes from the Python's documentation engine we rely upon, API docs are not mutually linked to each other unlike some other API docs e.g. javadoc.
Most of the commands are defined in the parent class hrpsys_config.HrpsysConfigurator
Hiro; hironx_ros_bridge/scripts/hironx.py
NEXTAGE OPEN; nextage_ros_bridge/script/nextage.py
Programming via Python I/F
Programming HiroNXO's using python can be done via an single integrated interface. Per robot the name of it differs:
* "HIRONX" for Hironx users. * "NextageClient" for NEXTAGE Open users.
In the following, we use "HIRONX" but same things should be possible for NEXTAGE Open users via NextageClient interface.
Sample code (1) Acceptance Test code re-visited
Let's look at a sample code and learn how to write your own application.
In Quick Start Guide, robot owners ran a set of actions. If you're on simulator, please go through the following portion (that is excepted from there) on your simulation environment, to see how it works first (in particular, "acceptance.run_tests_rtm()" is the one we'll look into in this tutorial:
Open one more terminal (this should be at least 3rd terminal opened), then open ipython again:
$ ipython -i `rospack find hironx_ros_bridge`/scripts/acceptancetest_hironx.py -- --host %HOSTNAME%
Then on ipython command line, run the sample that does TODO explain.
The following command runs these tasks via hrpsys interface:
IN [1]: acceptance.run_tests_rtm()
You can also run task-by-task:IN [1]: acceptance.run_tests_rtm(do_wait_input=True)
NOTE: If you're using hironx_ros_bridge version older than 1.0.20 (check by command $ rosversion hironx_ros_bridge), name of the command is different as following:
$ ipython -i `rospack find hironx_ros_bridge`/test/acceptancetest_hironx.py -- --host %HOSTNAME% IN [1]: acceptance_test.run_tests_hrpsys()
The following command runs these tasks via ROS interface:
IN [1]: acceptance.run_tests_ros()
When you're done with work on ipython, press Ctrl-d to escape its command line.
Let's look into the code.
First, this sample has 2-step dependency as in the image below.
The actions you just ran in the previous section is written in AcceptanceTest_Hiro class, which uses `AcceptancetestRTM` class to access HIRONX` class. What you really want to understand to write your own program is HIRONX class.
In short, let's look at AcceptanceTestRTM that directly accesses HIRONX.
If you're curious, the code of AcceptanceTest_Hiro can be found here. You might want to know about the 7 tasks being run, all of which are kicked from the _run_tests method.
Now here's the entire code:
1 # -*- coding: utf-8 -*-
2
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2014, TORK (Tokyo Opensource Robotics Kyokai Association)
6 # All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without
9 # modification, are permitted provided that the following conditions
10 # are met:
11 #
12 # * Redistributions of source code must retain the above copyright
13 # notice, this list of conditions and the following disclaimer.
14 # * Redistributions in binary form must reproduce the above
15 # copyright notice, this list of conditions and the following
16 # disclaimer in the documentation and/or other materials provided
17 # with the distribution.
18 # * Neither the name of TORK. nor the
19 # names of its contributors may be used to endorse or promote products
20 # derived from this software without specific prior written permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34
35 import time
36
37 from hironx_ros_bridge.constant import Constant
38 from hironx_ros_bridge.testutil.abst_acceptancetest import AbstAcceptanceTest
39
40
41 class AcceptanceTestRTM(AbstAcceptanceTest):
42
43 def __init__(self, robot_client):
44 '''
45 @type robot_client: hironx_ros_bridge.hironx_client.HIRONX
46 '''
47 self._robotclient = robot_client
48
49 def go_initpos(self):
50 self._robotclient.goInitial()
51
52 def set_joint_angles(self, joint_group, joint_angles, msg_tasktitle=None,
53 task_duration=7.0, do_wait=True):
54 '''
55 @see: AbstAcceptanceTest.set_joint_angles
56 '''
57 print("== RTM; {} ==".format(msg_tasktitle))
58 self._robotclient.setJointAnglesOfGroup(
59 joint_group, joint_angles, task_duration, do_wait)
60
61 def set_pose(self, joint_group, pose, rpy, msg_tasktitle,
62 task_duration=7.0, do_wait=True, ref_frame_name=None):
63
64 print("== RTM; {} ==".format(msg_tasktitle))
65 self._robotclient.setTargetPose(joint_group, pose, rpy, task_duration,
66 ref_frame_name)
67 if do_wait:
68 self._robotclient.waitInterpolationOfGroup(joint_group)
69
70 def set_pose_relative(
71 self, joint_group, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0,
72 msg_tasktitle=None, task_duration=7.0, do_wait=True):
73 if joint_group == Constant.GRNAME_LEFT_ARM:
74 eef = 'LARM_JOINT5'
75 elif joint_group == Constant.GRNAME_RIGHT_ARM:
76 eef = 'RARM_JOINT5'
77
78 print("== RTM; {} ==".format(msg_tasktitle))
79 self._robotclient.setTargetPoseRelative(
80 joint_group, eef, dx, dy, dz, dr, dp, dw,
81 task_duration, do_wait)
82
83 def _run_tests_hrpsys(self):
84 '''
85 @deprecated: This method remains as a reference. This used to function
86 when being called directly from ipython commandline and
87 now replaced by optimized codes.
88 '''
89 _TIME_SETTARGETP_L = 3
90 _TIME_SETTARGETP_R = 2
91 _TIME_BW_TESTS = 5
92
93 self.robot.goInitial()
94
95 # === TASK-1 ===
96 # L arm setTargetPose
97 _POS_L_INIT = self.robot.getCurrentPosition('LARM_JOINT5')
98 _POS_L_INIT[2] += 0.8
99 _RPY_L_INIT = self.robot.getCurrentRPY('LARM_JOINT5')
100 self.robot.setTargetPose('larm', _POS_L_INIT, _RPY_L_INIT, _TIME_SETTARGETP_L)
101 self.robot.waitInterpolationOfGroup('larm')
102
103 # R arm setTargetPose
104 _POS_R_INIT = self.robot.getCurrentPosition('RARM_JOINT5')
105 _POS_R_INIT[2] -= 0.07
106 _RPY_R_INIT = self.robot.getCurrentRPY('RARM_JOINT5')
107 self.robot.setTargetPose('rarm', _POS_R_INIT, _RPY_R_INIT, _TIME_SETTARGETP_R)
108 self.robot.waitInterpolationOfGroup('rarm')
109 time.sleep(_TIME_BW_TESTS)
110
111 # === TASK-2 ===
112 self.robot.goInitial()
113 # Both arm setTargetPose
114 _Z_SETTARGETP_L = 0.08
115 _Z_SETTARGETP_R = 0.08
116 self.robot.setTargetPoseRelative('larm', 'LARM_JOINT5',
117 dz=_Z_SETTARGETP_L,
118 tm=_TIME_SETTARGETP_L, wait=False)
119 self.robot.setTargetPoseRelative('rarm', 'RARM_JOINT5',
120 dz=_Z_SETTARGETP_R,
121 tm=_TIME_SETTARGETP_R, wait=False)
122
123 # === TASK-3 ===
124 # Head toward down
125 _TIME_HEAD = 5
126 self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dp=0.1, tm=_TIME_HEAD)
127 self.robot.waitInterpolationOfGroup('head')
128 # Head toward up
129 self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dp=-0.2, tm=_TIME_HEAD)
130 self.robot.waitInterpolationOfGroup('head')
131 # See left by position
132 self.robot.setJointAnglesOfGroup('head', [50, 10], 2, wait=True)
133 # See right by position
134 self.robot.setJointAnglesOfGroup('head', [-50, -10], 2, wait=True)
135 # Set back face to the starting pose w/o wait.
136 self.robot.setJointAnglesOfGroup( 'head', [0, 0], 2, wait=False)
137
138 # === TASK-4 ===
139 # 0.1mm increment is not working for some reason.
140 self.robot.goInitial()
141 # Move by iterating 0.1mm at cartesian space
142 _TIME_CARTESIAN = 0.1
143 _INCREMENT_MIN = 0.0001
144 for i in range(300):
145 self.robot.setTargetPoseRelative('larm', 'LARM_JOINT5',
146 dy=_INCREMENT_MIN,
147 tm=_TIME_CARTESIAN)
148 self.robot.setTargetPoseRelative('rarm', 'RARM_JOINT5',
149 dy=_INCREMENT_MIN,
150 tm=_TIME_CARTESIAN)
151 print('{}th move'.format(i))
152
153 self.robot.goInitial()
154 # === TASK-5 ===
155 # Turn torso
156 _TORSO_ANGLE = 120
157 _TIME_TORSO_R = 7
158 self.robot.setJointAnglesOfGroup('torso', [_TORSO_ANGLE], _TIME_TORSO_R, wait=True)
159 self.robot.waitInterpolationOfGroup('torso')
160 self.robot.setJointAnglesOfGroup('torso', [-_TORSO_ANGLE], 10, wait=True)
161
162 self.robot.goInitial()
163
164 # === TASK-6.1 ===
165 # Overwrite previous command, for torso using setJointAnglesOfGroup
166 self.robot.setJointAnglesOfGroup('torso', [_TORSO_ANGLE], _TIME_TORSO_R,
167 wait=False)
168 time.sleep(1)
169 self.robot.setJointAnglesOfGroup('torso', [-_TORSO_ANGLE], 10, wait=True)
170
171 self.robot.goInitial(5)
172
173 # === TASK-6.2 ===
174 # Overwrite previous command, for arms using setTargetPose
175 _X_EEF_OVERWRITE = 0.05
176 _Z_EEF_OVERWRITE = 0.1
177 _TIME_EEF_OVERWRITE = 7
178 _POS_L_INIT[0] += _X_EEF_OVERWRITE
179 _POS_L_INIT[2] += _Z_EEF_OVERWRITE
180 self.robot.setTargetPose('larm', _POS_L_INIT, _RPY_L_INIT, _TIME_EEF_OVERWRITE)
181 self.robot.waitInterpolationOfGroup('larm')
182 # Trying to raise rarm to the same level of larm.
183 _POS_R_INIT[0] += _X_EEF_OVERWRITE
184 _POS_R_INIT[2] += _Z_EEF_OVERWRITE
185 self.robot.setTargetPose('rarm', _POS_R_INIT, _RPY_R_INIT, _TIME_EEF_OVERWRITE)
186 self.robot.waitInterpolationOfGroup('rarm')
187 time.sleep(3)
188 # Stop rarm
189 self.robot.clearOfGroup('rarm') # Movement should stop here.
190
191 # === TASK-7.1 ===
192 # Cover wide workspace.
193 _TIME_COVER_WORKSPACE = 3
194 # Close to the max width the robot can spread arms with the hand kept
195 # at table level.
196 _POS_L_X_NEAR_Y_FAR = [0.32552812002303166, 0.47428609880442024, 1.0376656470275407]
197 _RPY_L_X_NEAR_Y_FAR = (-3.07491977663752, -1.5690249316560323, 3.074732073335767)
198 _POS_R_X_NEAR_Y_FAR = [0.32556456455769633, -0.47239119592815987, 1.0476131608682244]
199 _RPY_R_X_NEAR_Y_FAR = (3.072515432213872, -1.5690200270375372, -3.072326882451363)
200
201 # Close to the farthest distance the robot can reach, with the hand kept
202 # at table level.
203 _POS_L_X_FAR_Y_FAR = [0.47548142379781055, 0.17430276793604782, 1.0376878025614884]
204 _RPY_L_X_FAR_Y_FAR = (-3.075954857224205, -1.5690261926181046, 3.0757659493049574)
205 _POS_R_X_FAR_Y_FAR = [0.4755337947019357, -0.17242322190721648, 1.0476395479774052]
206 _RPY_R_X_FAR_Y_FAR = (3.0715850722714944, -1.5690204449882248, -3.071395243174742)
207 self.robot.setTargetPose('larm', _POS_L_X_NEAR_Y_FAR, _RPY_L_X_NEAR_Y_FAR, _TIME_COVER_WORKSPACE)
208 self.robot.setTargetPose('rarm', _POS_R_X_NEAR_Y_FAR, _RPY_R_X_NEAR_Y_FAR, _TIME_COVER_WORKSPACE)
209 self.robot.waitInterpolationOfGroup('larm')
210 self.robot.waitInterpolationOfGroup('rarm')
211 time.sleep(3)
212 self.robot.setTargetPose('larm', _POS_L_X_FAR_Y_FAR, _RPY_L_X_FAR_Y_FAR, _TIME_COVER_WORKSPACE)
213 self.robot.setTargetPose('rarm', _POS_R_X_FAR_Y_FAR, _RPY_R_X_FAR_Y_FAR, _TIME_COVER_WORKSPACE)
214 self.robot.waitInterpolationOfGroup('larm')
215 self.robot.waitInterpolationOfGroup('rarm')
216
217 self.robot.goInitial()
Let's look at a task as an example.
37 from hironx_ros_bridge.constant import Constant 38 from hironx_ros_bridge.testutil.abst_acceptancetest import AbstAcceptanceTest
You realize that HIRONX or NextageClient class, the RTM API we're all interested in, is not imported. No worries, it's taken care of in the following portion:
43 def __init__(self, robot_client): 44 ''' 45 @type robot_client: hironx_ros_bridge.hironx_client.HIRONX 46 ''' 47 self._robotclient = robot_client
Here, the constructor receives HIRONX and keeps it as a member self._robotclient. So in the rest of this tutorial self._robotclient points to an instance of HIRONX class. The remainder of the code is fundamental methods. For example:
49 def go_initpos(self): 50 self._robotclient.goInitial() 51
Simple enough, all joints move to the preset initial positions.Next,Later as you progress with your own code, you might want to learn options by looking at API doc of each class and method. In this tutorial we aren't going into detail much so please explore by yourself. For instance goInitial method has these options as follows:
- def hironx_ros_bridge.hironx_client.HIRONX.goInitial(self, tm = 7, wait = True, init_pose_type = 0)
52 def set_joint_angles(self, joint_group, joint_angles, msg_tasktitle=None, task_duration=7.0, do_wait=True): : 57 self._robotclient.setJointAnglesOfGroup(joint_group, joint_angles, task_duration, do_wait) 58
It's also pretty simply calling only the method of HIRONX where a lot of things are already handled so that you can write your code in a very simple way like this.
Sample code (2); Draw a circle with eef
Using the following code, you can draw a circle by the robot's end effector.
This is just a sample code. You're highly advised to tailor this code and to test virtually before you run it on your real robots.
robot variable needs to be somehow replaced with an instance of your Nextage_Client class.
def circle_eef(radius=0.01, eef='larm', step_degree=5, ccw=True, duration=0.1): ''' Moves the designated eef point-by-point so that the trajectory as a whole draws a circle. Currently this only works on the Y-Z plane of *ARM_JOINT5 joint. And it's the most intuitive when eef maintains a "goInitial" pose where circle gets drawn on robot's X-Y plane (see the wiki for the robot's coordinate if you're confused http://wiki.ros.org/rtmros_nextage/Tutorials/Programming#HiroNXO_3D_model_coordination). Points on the circular trajectory is based on a standard equation https://en.wikipedia.org/wiki/Circle#Equations @param radius: (Unit: meter) Radius of the circle to be drawn. @param step_degree: Angle in degree each iteration increments. @param ccw: counter clock-wise. @param duration: Time for each iteration to be completed. ''' goal_deg = GOAL_DEGREE = 360 start_deg = 0 if eef == 'larm': joint_eef = 'LARM_JOINT5' elif eef == 'rarm': joint_eef = 'RARM_JOINT5' eef_pos = robot.getCurrentPosition(joint_eef) eef_rpy = robot.getCurrentRPY(joint_eef) print('eef_pos={}'.format(eef_pos)) X0 = eef_pos[0] Y0 = eef_pos[1] ORIGIN_x = X0 ORIGIN_y = Y0 - radius print('ORIGIN_x={} ORIGIN_y={}'.format(ORIGIN_x, ORIGIN_y)) i = 0 for theta in range(start_deg, goal_deg, step_degree): if not ccw: theta = -theta x = ORIGIN_x + radius*math.sin(math.radians(theta)) # x-axis in robot's eef space is y in x-y graph y = ORIGIN_y + radius*math.cos(math.radians(theta)) eef_pos[0] = x eef_pos[1] = y print('#{}th theta={} x={} y={} X0={} Y0={}'.format(i, theta, x, y, X0, Y0)) robot.setTargetPose(eef, eef_pos, eef_rpy, duration) robot.waitInterpolation() i += 1
Usacase-oriented programming
To not override actions
By default, some of the commands in HIRONX class wait for the commanded action ends before the next action starts. Some don't. It depends on the implementation that is clear from the arguments each method takes. Methods with wait argument are capable of waiting or not. Others are not, unless mentioned in their API docs. Example of those that have "wait" feature:
70 def set_pose_relative(self, joint_group, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, msg_tasktitle=None, task_duration=7.0, do_wait=True): 73 if joint_group == Constant.GRNAME_LEFT_ARM: 74 eef = 'LARM_JOINT5' 75 elif joint_group == Constant.GRNAME_RIGHT_ARM: 76 eef = 'RARM_JOINT5' : 79 self._robotclient.setTargetPoseRelative( 80 joint_group, eef, dx, dy, dz, dr, dp, dw, task_duration, do_wait)
Unless you want, generally it would be safe to set wait=True by default in your codes.
In the following, setTargetPose doesn't take any wait / interruption signals. In that case you could work it around by calling HrpsysConfigurator.waitInterpolationOfGroup() (HrpsysConfigurator is the parent class of HIRONX).
61 def set_pose(self, joint_group, position, rpy, msg_tasktitle, task_duration=7.0, do_wait=True, ref_frame_name=None): : 65 self._robotclient.setTargetPose(joint_group, position, rpy, task_duration, ref_frame_name) 67 if do_wait: 68 self._robotclient.waitInterpolationOfGroup(joint_group) 68
Sending continuous trajectory commands
Discussed here. Please ask for the detail there if necessary.
hcf.playPatternOfGroup('LARM', [[0.010,0.0,-1.745,-0.265,0.164,0.06], [0.010,-0.2,-2.045,-0.265,0.164,0.06], [0.010,-0.4,-2.245,-0.265,0.164,0.06], [0.010,-0.6,-2.445,-0.265,0.164,0.06], [0.010,-0.8,-2.645,-0.265,0.164,0.06]], [1,1,1,1,1])
hcf can be robot (depends on how you boot python interface).
From hrpsys 315.6.0, you can use setJointAnglesSequenceOfGroup.
Relative movement using setTargetPoseRelative command
Moving EEFs or any joints by passing delta value (change from the current status) is made easy with HIRONX interface.
- Example1: In the following, [1] moving torso 0.1 rad over 3 second, [2] moving EEF at right hand toward front 10 centimeters:
In [1]: robot.setTargetPoseRelative('torso', 'CHEST_JOINT0', dw=0.1, tm=3) In [2]: robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dx=0.1, tm=3)
To use your custom Python code that uses hrpsys commands
In this page up to here, we've been talking about how to use the methods provided by the robot's client interface class (HIRONX / NextageClient). There you ran a script hironx.py or nextage.py to instantiate the interface class.
Now, let's assume that you created your own application module by integrating the methods from client interface class. Say you named it your_nxo_sample.py. Now you want to run it against the robot (either on simulation or on the real robot).
One way is to simply duplicate nextage.py, and replace the line where NextageClient class is instantiated (also remove the rest of the file where the instance of NextageClient is accessed).
The above way not smart nor clean because you simply duplicate the majority of the code, which is a bad practice in software development. As far as the author knows, hrpsys_tools/hrpsys_tools_config.py is intended to serve for this purpose (see also start-jsk/rtmros_common#340 for the discussion when it was created), but unfortunately there has been no report on how to use that with HiroNXO yet (as of 1/30/2017).