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

complete http://wiki.ros.org/rtmros_nextage/Tutorials/Task-oriented%20programming#Using_RTM_interface_commands #312

Closed k-okada closed 7 years ago

k-okada commented 7 years ago

I'd like to move robot specifing a list of waypoint (lisst of target poses) for example, rectangle or circular motion on some plane surface Information written http://wiki.ros.org/rtmros_nextage/Tutorials/Task-oriented%20programming#Using_RTM_interface_commands is imcomplete

# For larm for example.
# Define the sequence of positions and RPYs of the waypoints.
positions_larm = [pos1, pos2,...,posn]  # All elements are lists. E.g. pos1 = [x1, y1, z1]
rpys_larm = [rpy1, rpy2,...,rpyn]       # All elements are lists. E.g. rpy1 = [r1, p1, y1]

play_pattern_larm = []
for pos, rpy in zip(positions_larm, rpys_larm):
    robot.setTargetPose('larm', pos, rpy, tm=5)            # Pass necessary args.
    # Example for left arm. For right arm use [3:9] instead.
    play_pattern_larm.append(robot.getJointAngles()[10:])

We'd need working example.

  1. add working example of pos1, pos2...rpy1, rpy2, may be rectangle and circular motions
  2. call robot.playPattern in the end
  3. define setTargetPoseSequence(limb, pos_list, rpy_list, tm_list), to readability
y-yosuke commented 7 years ago

robot.playPattern() does not work after executing robot.goInitial().

As in an example below, robot.playPattern() works in lines from In [1]: to In [7]: , but after execting robot.goInitial() at In [8]: , at In [9]: robot.playPattern() does not work (nothing executed).

Is there any procedure missing?

$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py 
Python 2.7.6 (default, Oct 26 2016, 20:30:19) 
Type "copyright", "credits" or "license" for more information.

IPython 1.2.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 robotuser-PC: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 HiroNX(Robot)0 : <hrpsys.rtm.RTcomponent instance at 0x7f5641a8aef0> ( timeout 0 < 10)
[hrpsys.py] findComps -> RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7f5641a8aef0> isActive? = True 
[hrpsys.py] simulation_mode : True
[hrpsys.py]  Hrpsys controller version info: 
[hrpsys.py]    ms =  <hrpsys.rtm.RTCmanager instance at 0x7f5641f93248>
[hrpsys.py]    ref =  <RTM._objref_Manager instance at 0x7f5641f93170>
[hrpsys.py]    version  =  315.10.1
[hrpsys.py] hrpsys components are already created and running
[hrpsys.py] [self.seq, self.seq_svc, self.seq_version] = self.findComp("SequencePlayer","seq",0,True)
[hrpsys.py]  find Comp    : seq = <hrpsys.rtm.RTcomponent instance at 0x7f5641a8f7a0> (315.10.1)
[hrpsys.py]  find CompSvc : seq_svc = <OpenHRP._objref_SequencePlayerService instance at 0x7f56410a7998>
[hrpsys.py] [self.sh, self.sh_svc, self.sh_version] = self.findComp("StateHolder","sh",0,True)
[hrpsys.py]  find Comp    : sh = <hrpsys.rtm.RTcomponent instance at 0x7f5641a9aef0> (315.10.1)
[hrpsys.py]  find CompSvc : sh_svc = <OpenHRP._objref_StateHolderService instance at 0x7f56410d9ea8>
[hrpsys.py] [self.fk, self.fk_svc, self.fk_version] = self.findComp("ForwardKinematics","fk",0,True)
[hrpsys.py]  find Comp    : fk = <hrpsys.rtm.RTcomponent instance at 0x7f56410ece18> (315.10.1)
[hrpsys.py]  find CompSvc : fk_svc = <OpenHRP._objref_ForwardKinematicsService instance at 0x7f5641102950>
[hrpsys.py] [self.ic, self.ic_svc, self.ic_version] = self.findComp("ImpedanceController","ic",0,True)
[hrpsys.py]  find Comp    : ic = <hrpsys.rtm.RTcomponent instance at 0x7f564110ba28> (315.10.1)
[hrpsys.py]  find CompSvc : ic_svc = <OpenHRP._objref_ImpedanceControllerService instance at 0x7f5641f93d40>
[hrpsys.py] [self.el, self.el_svc, self.el_version] = self.findComp("SoftErrorLimiter","el",0,True)
[hrpsys.py]  find Comp    : el = <hrpsys.rtm.RTcomponent instance at 0x7f5641102ab8> (315.10.1)
[hrpsys.py]  find CompSvc : el_svc = <OpenHRP._objref_SoftErrorLimiterService instance at 0x7f5641a877e8>
[hrpsys.py] [self.sc, self.sc_svc, self.sc_version] = self.findComp("ServoController","sc",0,True)
[hrpsys.py]  find Comp    : sc = <hrpsys.rtm.RTcomponent instance at 0x7f56410ec290> (315.10.1)
[hrpsys.py]  find CompSvc : sc_svc = <OpenHRP._objref_ServoControllerService instance at 0x7f5641102d40>
[hrpsys.py] [self.log, self.log_svc, self.log_version] = self.findComp("DataLogger","log",0,True)
[hrpsys.py]  find Comp    : log = <hrpsys.rtm.RTcomponent instance at 0x7f564110b680> (315.10.1)
[hrpsys.py]  find CompSvc : log_svc = <OpenHRP._objref_DataLoggerService instance at 0x7f56410a7f80>
[hrpsys.py] [self.rmfo, self.rmfo_svc, self.rmfo_version] = self.findComp("RemoveForceSensorLinkOffset","rmfo",0,True)
[hrpsys.py]  find Comp    : rmfo = <hrpsys.rtm.RTcomponent instance at 0x7f5641a89830> (315.10.1)
[hrpsys.py]  find CompSvc : rmfo_svc = <OpenHRP._objref_RemoveForceSensorLinkOffsetService instance at 0x7f56411025f0>
[hrpsys.py] setup joint groups for hrpsys controller
[hrpsys.py] initialized successfully
[ INFO] [1493117365.556798679]: Loading robot model 'NextageOpen'...
[INFO] [WallTime: 1493117367.682923] [25.350000] 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] [1493117368.597603644, 26.294999999]: TrajectoryExecution will use old service capability.
[ INFO] [1493117368.597750506, 26.294999999]: Ready to take MoveGroup commands for group left_arm.
[ INFO] [1493117369.103810229, 26.919999999]: TrajectoryExecution will use old service capability.
[ INFO] [1493117369.103989385, 26.919999999]: Ready to take MoveGroup commands for group right_arm.
[ INFO] [1493117369.923120865, 27.654999999]: TrajectoryExecution will use old service capability.
[ INFO] [1493117369.923294751, 27.654999999]: Ready to take MoveGroup commands for group botharms.
[ INFO] [1493117370.625991668, 28.319999999]: TrajectoryExecution will use old service capability.
[ INFO] [1493117370.626158374, 28.319999999]: Ready to take MoveGroup commands for group head.
[ INFO] [1493117371.092244517, 28.919999999]: TrajectoryExecution will use old service capability.
[ INFO] [1493117371.092360789, 28.919999999]: Ready to take MoveGroup commands for group torso.
[ INFO] [1493117371.809318927, 29.524999999]: TrajectoryExecution will use old service capability.
[ INFO] [1493117371.809422875, 29.524999999]: Ready to take MoveGroup commands for group upperbody.

In [1]: robot.playPattern([[0, 1,  0,  0, 0.3, -1.9, 0, 0, 0,  0, 0.3, -1.9, 0, 0]], [], [], [1.0])

In [2]: robot.playPattern([[0, -1,  0,  0, 0.3, -1.9, 0, 0, 0,  0, 0.3, -1.9, 0, 0]], [], [], [1.0])

In [3]: robot.playPattern([[0, 1,  0,  0, 0.3, -1.9, 0, 0, 0,  0, 0.3, -1.9, 0, 0]], [], [], [1.0])

In [4]: robot.playPattern([[0, -1,  0,  0, 0.3, -1.9, 0, 0, 0,  0, 0.3, -1.9, 0, 0]], [], [], [1.0])

In [5]: robot.playPattern([[0, 1,  0,  0, 0.3, -1.9, 0, 0, 0,  0, 0.3, -1.9, 0, 0]], [], [], [1.0])

In [6]: robot.playPattern([[0, 1,  0,  0, 0.3, -1.9, 0, 0, 0,  0, 0.3, -1.9, 0, 0]], [], [], [1.0])

In [7]: robot.playPattern([[0, -1,  0,  0, 0.3, -1.9, 0, 0, 0,  0, 0.3, -1.9, 0, 0]], [], [], [1.0])

In [8]: robot.goInitial()
[hrpsys.py] , JntAnglesOfGr=torso, INITPOSE[i]=[0], tm=7, wait=True
[hrpsys.py] , JntAnglesOfGr=head, INITPOSE[i]=[0, 0], tm=7, wait=True
[hrpsys.py] , JntAnglesOfGr=rarm, INITPOSE[i]=[-0.6, 0, -100, 15.2, 9.4, 3.2], tm=7, wait=True
[hrpsys.py] , JntAnglesOfGr=larm, INITPOSE[i]=[0.6, 0, -100, -15.2, 9.4, -3.2], tm=7, wait=True
Out[8]: True

In [9]: robot.playPattern([[0, -1,  0,  0, 0.3, -1.9, 0, 0, 0,  0, 0.3, -1.9, 0, 0]], [], [], [1.0])

In [10]:
y-yosuke commented 7 years ago

robot.playPatternOfGroup() works well after executing robot.goInitial(). 

$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py 
Python 2.7.6 (default, Oct 26 2016, 20:30:19) 
Type "copyright", "credits" or "license" for more information.

IPython 1.2.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 robotuser-PC: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 HiroNX(Robot)0 : <hrpsys.rtm.RTcomponent instance at 0x7f11deb2cef0> ( timeout 0 < 10)
[hrpsys.py] findComps -> RobotHardware : <hrpsys.rtm.RTcomponent instance at 0x7f11deb2cef0> isActive? = True 
[hrpsys.py] simulation_mode : True
[hrpsys.py]  Hrpsys controller version info: 
[hrpsys.py]    ms =  <hrpsys.rtm.RTCmanager instance at 0x7f11ddde3248>
[hrpsys.py]    ref =  <RTM._objref_Manager instance at 0x7f11ddde3170>
[hrpsys.py]    version  =  315.10.1
[hrpsys.py] hrpsys components are already created and running
[hrpsys.py] [self.seq, self.seq_svc, self.seq_version] = self.findComp("SequencePlayer","seq",0,True)
[hrpsys.py]  find Comp    : seq = <hrpsys.rtm.RTcomponent instance at 0x7f11deb25bd8> (315.10.1)
[hrpsys.py]  find CompSvc : seq_svc = <OpenHRP._objref_SequencePlayerService instance at 0x7f11de237a70>
[hrpsys.py] [self.sh, self.sh_svc, self.sh_version] = self.findComp("StateHolder","sh",0,True)
[hrpsys.py]  find Comp    : sh = <hrpsys.rtm.RTcomponent instance at 0x7f11de1dbab8> (315.10.1)
[hrpsys.py]  find CompSvc : sh_svc = <OpenHRP._objref_StateHolderService instance at 0x7f11dec25170>
[hrpsys.py] [self.fk, self.fk_svc, self.fk_version] = self.findComp("ForwardKinematics","fk",0,True)
[hrpsys.py]  find Comp    : fk = <hrpsys.rtm.RTcomponent instance at 0x7f11deb2bbd8> (315.10.1)
[hrpsys.py]  find CompSvc : fk_svc = <OpenHRP._objref_ForwardKinematicsService instance at 0x7f11dec07bd8>
[hrpsys.py] [self.ic, self.ic_svc, self.ic_version] = self.findComp("ImpedanceController","ic",0,True)
[hrpsys.py]  find Comp    : ic = <hrpsys.rtm.RTcomponent instance at 0x7f11deb29bd8> (315.10.1)
[hrpsys.py]  find CompSvc : ic_svc = <OpenHRP._objref_ImpedanceControllerService instance at 0x7f11de22a908>
[hrpsys.py] [self.el, self.el_svc, self.el_version] = self.findComp("SoftErrorLimiter","el",0,True)
[hrpsys.py]  find Comp    : el = <hrpsys.rtm.RTcomponent instance at 0x7f11df809368> (315.10.1)
[hrpsys.py]  find CompSvc : el_svc = <OpenHRP._objref_SoftErrorLimiterService instance at 0x7f11dec073b0>
[hrpsys.py] [self.sc, self.sc_svc, self.sc_version] = self.findComp("ServoController","sc",0,True)
[hrpsys.py]  find Comp    : sc = <hrpsys.rtm.RTcomponent instance at 0x7f11deb2b3f8> (315.10.1)
[hrpsys.py]  find CompSvc : sc_svc = <OpenHRP._objref_ServoControllerService instance at 0x7f11dddc5200>
[hrpsys.py] [self.log, self.log_svc, self.log_version] = self.findComp("DataLogger","log",0,True)
[hrpsys.py]  find Comp    : log = <hrpsys.rtm.RTcomponent instance at 0x7f11de1db9e0> (315.10.1)
[hrpsys.py]  find CompSvc : log_svc = <OpenHRP._objref_DataLoggerService instance at 0x7f11de237e18>
[hrpsys.py] [self.rmfo, self.rmfo_svc, self.rmfo_version] = self.findComp("RemoveForceSensorLinkOffset","rmfo",0,True)
[hrpsys.py]  find Comp    : rmfo = <hrpsys.rtm.RTcomponent instance at 0x7f11de1e8e60> (315.10.1)
[hrpsys.py]  find CompSvc : rmfo_svc = <OpenHRP._objref_RemoveForceSensorLinkOffsetService instance at 0x7f11deb2c8c0>
[hrpsys.py] setup joint groups for hrpsys controller
[hrpsys.py] initialized successfully
[ INFO] [1493120528.388230141]: Loading robot model 'NextageOpen'...
[INFO] [WallTime: 1493120531.047053] [196.780000] 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] [1493120532.307302069, 198.024999999]: TrajectoryExecution will use old service capability.
[ INFO] [1493120532.307575157, 198.024999999]: Ready to take MoveGroup commands for group left_arm.
[ INFO] [1493120532.967368920, 198.659999999]: TrajectoryExecution will use old service capability.
[ INFO] [1493120532.967482379, 198.659999999]: Ready to take MoveGroup commands for group right_arm.
[ INFO] [1493120533.772431276, 199.499999999]: TrajectoryExecution will use old service capability.
[ INFO] [1493120533.772506981, 199.499999999]: Ready to take MoveGroup commands for group botharms.
[ERROR] [WallTime: 1493120538.788865] [199.505000] Unable to connect to move_group action server 'place' within allotted time (5s)
Make sure you've launched MoveGroup (e.g. by launching moveit_planning_execution.launch)

In [1]: robot.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])
Out[1]: True

In [2]: robot.goInitial()
[hrpsys.py] , JntAnglesOfGr=torso, INITPOSE[i]=[0], tm=7, wait=True
[hrpsys.py] , JntAnglesOfGr=head, INITPOSE[i]=[0, 0], tm=7, wait=True
[hrpsys.py] , JntAnglesOfGr=rarm, INITPOSE[i]=[-0.6, 0, -100, 15.2, 9.4, 3.2], tm=7, wait=True
[hrpsys.py] , JntAnglesOfGr=larm, INITPOSE[i]=[0.6, 0, -100, -15.2, 9.4, -3.2], tm=7, wait=True
Out[2]: True

In [3]: robot.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])
Out[3]: True

In [4]: exit
y-yosuke commented 7 years ago

I wrote an elemental example of a rectangular motion pattern using playPatternOfGroup() as below.

I will define setTargetPoseSequence() and add a motion of circular pattern later.

#!/usr/bin/env python
# -*- coding: utf-8 -*-

# Software License Agreement (BSD License)
#
# Copyright (c) 2013, Tokyo Opensource Robotics Kyokai Association
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Tokyo Opensource Robotics Kyokai Association. nor the
#    names of its contributors may be used to endorse or promote products
#    derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Isaac Isao Saito

# Author: Yosuke Yamamoto

import socket

from hironx_ros_bridge.ros_client import ROS_Client
# This should come earlier than later import.
# See http://code.google.com/p/rtm-ros-robotics/source/detail?r=6773
from nextage_ros_bridge import nextage_client
from rospy import ROSInitException

from hrpsys import rtm
import argparse

errormsg_noros = 'No ROS Master found. Without it, you cannot use ROS from' \
                 ' this script, but can use RTM. 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'

import math

if __name__ == '__main__':
    parser = argparse.ArgumentParser(description='hiro command line interpreters')
    parser.add_argument('--host', help='corba name server hostname')
    parser.add_argument('--port', help='corba name server port number')
    parser.add_argument('--modelfile', help='robot model file nmae')
    parser.add_argument('--robot', help='robot modlule name (RobotHardware0 for real robot, Robot()')
    parser.add_argument('--dio_ver', help="Version of digital I/O. Only users "
                        "whose robot was shipped before Aug 2014 need to "
                        "define this, and the value should be '0.4.2'.")
    args, unknown = parser.parse_known_args()

    if args.host:
        rtm.nshost = args.host
    if args.port:
        rtm.nsport = args.port
    if not args.robot:
        args.robot = "RobotHardware0" if args.host else "HiroNX(Robot)0"
    if not args.modelfile:
        args.modelfile = "/opt/jsk/etc/NEXTAGE/model/main.wrl" if args.host else "" 

    # support old style format
    if len(unknown) >= 2:
        args.robot = unknown[0]
        args.modelfile = unknown[1]
    robot = nxc = nextage_client.NextageClient()
    # Use generic name for the robot instance. This enables users on the
    # script commandline (eg. ipython) to run the same commands without asking
    # them to specifically tell what robot they're using (eg. hiro, nxc).
    # This is backward compatible so that users can still keep using `nxc`.
    # See http://code.google.com/p/rtm-ros-robotics/source/detail?r=6926
    robot.init(robotname=args.robot, url=args.modelfile)

    if args.dio_ver:
        robot.set_hand_version(args.dio_ver)

    try:
        ros = ROS_Client()
    except ROSInitException as e:
        print('[nextage.py] {}'.format(e))
    except socket.error as e: 
        print("\033[31m%s\n%s\033[0m" % (e.strerror, errormsg_noros))

    robot.goInitial()

    # For larm for example.
    # Define the sequence of positions and RPYs of the waypoints.
    motion_time = 3.0
    positions_larm = [
                       [0.25, 0.0, 0.1], 
                       [0.25, 0.2, 0.1],
                       [0.45, 0.2, 0.1],
                       [0.45, 0.0, 0.1],
                       [0.25, 0.0, 0.1]
                     ]  # All elements are lists. E.g. pos1 = [x1, y1, z1]
    rpys_larm = [
                  [-3.0734370292070254, -1.5690226177313638, 3.073247044207053], 
                  [-3.0734370292070254, -1.5690226177313638, 3.073247044207053],
                  [-3.0734370292070254, -1.5690226177313638, 3.073247044207053],
                  [-3.0734370292070254, -1.5690226177313638, 3.073247044207053],
                  [-3.0734370292070254, -1.5690226177313638, 3.073247044207053]
                ]       # All elements are lists. E.g. rpy1 = [r1, p1, y1]

    # Generationg Pattern
    print('Generating Pattern')
    play_pattern_larm = []
    play_pattern_time = []
    for pos, rpy in zip(positions_larm, rpys_larm):
        robot.setTargetPose('larm', pos, rpy, tm=motion_time)
        robot.waitInterpolationOfGroup('larm')
        joint_angles_deg = robot.getJointAngles()[9:]
        joint_angles_rad = [math.radians(angle_in_degree) for angle_in_degree in joint_angles_deg]
        play_pattern_larm.append(joint_angles_rad)
        play_pattern_time.append(motion_time)

    print('Pattern Generated')
    print('Joint Angle Pattern [rad]: %s' % play_pattern_larm)
    print('Motion Time Pattern [sec]: %s' % play_pattern_time)

    robot.goInitial()

    # Play Pattern
    print('Start: Play Pattern')
    robot.playPatternOfGroup('larm', play_pattern_larm, play_pattern_time)
    robot.waitInterpolationOfGroup('larm')
    print('End: Play Pattern')

    robot.goInitial()

# for simulated robot
# $ ./hironx.py
#
# for real robot
# ./hironx.py  --host hiro014
# ./ipython -i hironx.py --host hiro014
# for real robot with custom model file
# ./hironx.py  --host hiro014 --modelfile /opt/jsk/etc/NEXTAGE/model/main.wrl
y-yosuke commented 7 years ago

I wrote a playPatternOfGroup() example file nextage_playpattern.py as shown below.

I have confirmed the operation of this file in the simulation.

When it seems that this file is good after some modifications, would it be better to put the file in any extisting or new package? or I just paste it on the document of http://wiki.ros.org/rtmros_nextage/Tutorials/Task-oriented%20programming#Using_RTM_interface_commands ?

#!/usr/bin/env python
# -*- coding: utf-8 -*-

# Software License Agreement (BSD License)
#
# Copyright (c) 2017, Tokyo Opensource Robotics Kyokai Association
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Tokyo Opensource Robotics Kyokai Association. nor the
#    names of its contributors may be used to endorse or promote products
#    derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Isaac Isao Saito
# Author: Yosuke Yamamoto

import socket

from hironx_ros_bridge.ros_client import ROS_Client
# This should come earlier than later import.
# See http://code.google.com/p/rtm-ros-robotics/source/detail?r=6773
from nextage_ros_bridge import nextage_client
from rospy import ROSInitException

from hrpsys import rtm
import argparse

errormsg_noros = 'No ROS Master found. Without it, you cannot use ROS from' \
                 ' this script, but can use RTM. 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'

import math

def circularPositions(center=[0.3, 0.2, 0.1], radius=0.1 ,steps=12):
    positions_xyz = []
    step_rad = 2 * math.pi / steps
    for i in range(steps+1):
        ang_rad = step_rad * i
        px = center[0] - radius * math.cos(ang_rad)
        py = center[1] + radius * math.sin(ang_rad)
        pz = center[2]
        positions_xyz.append([px,py,pz])

    return positions_xyz

def rectangularPositions(dp_a=[0.25, 0.0, 0.1], dp_b=[0.45, 0.2, 0.1]):
    positions_xyz = [
                     [ dp_a[0], dp_a[1], dp_a[2] ],
                     [ dp_a[0], dp_b[1], dp_a[2] ],
                     [ dp_b[0], dp_b[1], dp_b[2] ],
                     [ dp_b[0], dp_a[1], dp_b[2] ],
                     [ dp_a[0], dp_a[1], dp_a[2] ]
                    ]

    return positions_xyz

def samePostureRPY(rpy, pat_length):
    posture_rpy = []
    for i in range(pat_length):
        posture_rpy.append(rpy)

    return posture_rpy

def equalTimeList(whole_tm, pat_length):
    tm_list = []
    tm = whole_tm / pat_length
    for i in range(pat_length):
        tm_list.append(tm)

    return tm_list

def setTargetPoseSequence(limb, pos_list, rpy_list, tm_list):
    pattern_arm = []
    for pos, rpy, tm in zip(pos_list, rpy_list, tm_list):
        robot.setTargetPose(limb, pos, rpy, tm)
        robot.waitInterpolationOfGroup(limb)
        if limb == 'rarm':
            joint_angles_deg = robot.getJointAngles()[3:9]
        else:
            joint_angles_deg = robot.getJointAngles()[9:]
        joint_angles_rad = [math.radians(angle_in_degree) for angle_in_degree in joint_angles_deg]
        pattern_arm.append(joint_angles_rad)

    return pattern_arm

if __name__ == '__main__':
    parser = argparse.ArgumentParser(description='hiro command line interpreters')
    parser.add_argument('--host', help='corba name server hostname')
    parser.add_argument('--port', help='corba name server port number')
    parser.add_argument('--modelfile', help='robot model file nmae')
    parser.add_argument('--robot', help='robot modlule name (RobotHardware0 for real robot, Robot()')
    parser.add_argument('--dio_ver', help="Version of digital I/O. Only users "
                        "whose robot was shipped before Aug 2014 need to "
                        "define this, and the value should be '0.4.2'.")
    args, unknown = parser.parse_known_args()

    if args.host:
        rtm.nshost = args.host
    if args.port:
        rtm.nsport = args.port
    if not args.robot:
        args.robot = "RobotHardware0" if args.host else "HiroNX(Robot)0"
    if not args.modelfile:
        args.modelfile = "/opt/jsk/etc/NEXTAGE/model/main.wrl" if args.host else "" 

    # support old style format
    if len(unknown) >= 2:
        args.robot = unknown[0]
        args.modelfile = unknown[1]
    robot = nxc = nextage_client.NextageClient()
    # Use generic name for the robot instance. This enables users on the
    # script commandline (eg. ipython) to run the same commands without asking
    # them to specifically tell what robot they're using (eg. hiro, nxc).
    # This is backward compatible so that users can still keep using `nxc`.
    # See http://code.google.com/p/rtm-ros-robotics/source/detail?r=6926
    robot.init(robotname=args.robot, url=args.modelfile)

    if args.dio_ver:
        robot.set_hand_version(args.dio_ver)

    try:
        ros = ROS_Client()
    except ROSInitException as e:
        print('[nextage.py] {}'.format(e))
    except socket.error as e: 
        print("\033[31m%s\n%s\033[0m" % (e.strerror, errormsg_noros))

    # playPatternOfGroup() Example
    robot.goInitial()

    positions_arm = []  # All elements are lists. E.g. pos1 = [x1, y1, z1]
    rpys_arm = []       # All elements are lists. E.g. rpy1 = [r1, p1, y1]
    time_list = []      # Time list [t1, t2, t3,...]
    limb_name = 'larm'  # Limb 'rarm', 'larm'

    # Rectangular Positions, RPYs, Time List
    rect_xyzs = rectangularPositions(dp_a=[0.25, 0.0, 0.1], dp_b=[0.45, 0.2, 0.1])
    rect_rpys = samePostureRPY([-3.073437, -1.569023, 3.073247], len(rect_xyzs))
    rect_time = equalTimeList(10.0, len(rect_xyzs))

    # Circular Positions, RPYs, Time List
    circ_xyzs = circularPositions(center=[0.35, 0.1, 0.1], radius=0.1 ,steps=12)
    circ_rpys = samePostureRPY([-3.073437, -1.569023, 3.073247], len(circ_xyzs))
    circ_time = equalTimeList(10.0, len(circ_xyzs))

    # Adjust Transition Time
    rect_time[0] += 1.0
    circ_time[0] += 1.0

    # Combine Patterns
    positions_arm = rect_xyzs + circ_xyzs
    rpys_arm      = rect_rpys + circ_rpys
    time_list     = rect_time + circ_time

    print('Limb: %s' % limb_name)
    print('Positions (%d): %s' % (len(positions_arm), positions_arm))
    print('RPY Pattern (%d): %s' % (len(rpys_arm), rpys_arm))
    print('Time List (%d): %s' % (len(time_list), time_list))

    # Generate Joint Angle Pattern with Moving
    print('Generating Joint Angle Pattern')
    play_pattern_arm = setTargetPoseSequence(limb_name, positions_arm, rpys_arm, time_list)
    play_pattern_time = time_list

    print('Generated')
    print('Limb: %s' % limb_name)
    print('Joint Angle Pattern [rad]: %s' % play_pattern_arm)
    print('Motion Time Pattern [sec]: %s' % play_pattern_time)

    robot.goInitial()

    # Play Pattern - playPatternOfGroup()
    print('Start: Play Pattern')
    robot.playPatternOfGroup(limb_name, play_pattern_arm, play_pattern_time)
    robot.waitInterpolationOfGroup(limb_name)
    print('End: Play Pattern')

    robot.goInitial()

# for simulated robot
# $ ./hironx.py
#
# for real robot
# ./hironx.py  --host hiro014
# ./ipython -i hironx.py --host hiro014
# for real robot with custom model file
# ./hironx.py  --host hiro014 --modelfile /opt/jsk/etc/NEXTAGE/model/main.wrl
y-yosuke commented 7 years ago

I added a description about the playPattern example in ROS Wiki. http://wiki.ros.org/rtmros_nextage/Tutorials/Task-oriented%20programming#Using_RTM