{"action":"created","issue":{"url":"https://api.github.com/repos/tork-a/rtmros_nextage/issues/312","repository_url":"https://api.github.com/repos/tork-a/rtmros_nextage","labels_url":"https://api.github.com/repos/tork-a/rtmros_nextage/issues/312/labels{/name}","comments_url":"https://api.github.com/repos/tork-a/rtmros_nextage/issues/312/comments","events_url":"https://api.github.com/repos/tork-a/rtmros_nextage/issues/312/events","html_url":"https://github.com/tork-a/rtmros_nextage/issues/312","id":223675605,"number":312,"title":"complete http://wiki.ros.org/rtmros_nextage/Tutorials/Task-oriented%20programming#Using_RTM_interface_commands","user":{"login":"k-okada","id":493276,"avatar_url":"https://avatars3.githubusercontent.com/u/493276?v=3","gravatar_id":"","url":"https://api.github.com/users/k-okada","html_url":"https://github.com/k-okada","followers_url":"https://api.github.com/users/k-okada/followers","following_url":"https://api.github.com/users/k-okada/following{/other_user}","gists_url":"https://api.github.com/users/k-okada/gists{/gist_id}","starred_url":"https://api.github.com/users/k-okada/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/k-okada/subscriptions","organizations_url":"https://api.github.com/users/k-okada/orgs","repos_url":"https://api.github.com/users/k-okada/repos","events_url":"https://api.github.com/users/k-okada/events{/privacy}","received_events_url":"https://api.github.com/users/k-okada/received_events","type":"User","site_admin":false},"labels":[],"state":"open","locked":false,"assignee":null,"assignees":[],"milestone":null,"comments":3,"created_at":"2017-04-23T23:27:36Z","updated_at":"2017-04-26T10:09:54Z","closed_at":null,"body":"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\r\nInformation written \r\nhttp://wiki.ros.org/rtmros_nextage/Tutorials/Task-oriented%20programming#Using_RTM_interface_commands is imcomplete\r\n\r\n```\r\n# For larm for example.\r\n# Define the sequence of positions and RPYs of the waypoints.\r\npositions_larm = [pos1, pos2,...,posn]  # All elements are lists. E.g. pos1 = [x1, y1, z1]\r\nrpys_larm = [rpy1, rpy2,...,rpyn]       # All elements are lists. E.g. rpy1 = [r1, p1, y1]\r\n\r\nplay_pattern_larm = []\r\nfor pos, rpy in zip(positions_larm, rpys_larm):\r\n    robot.setTargetPose('larm', pos, rpy, tm=5)            # Pass necessary args.\r\n    # Example for left arm. For right arm use [3:9] instead.\r\n    play_pattern_larm.append(robot.getJointAngles()[10:])\r\n```\r\nWe'd need working example.\r\n1. add working example of pos1, pos2...rpy1, rpy2, may be rectangle and circular motions\r\n2. call robot.playPattern in the end\r\n3. define setTargetPoseSequence(limb, pos_list, rpy_list, tm_list), to readability\r\n"},"comment":{"url":"https://api.github.com/repos/tork-a/rtmros_nextage/issues/comments/297333313","html_url":"https://github.com/tork-a/rtmros_nextage/issues/312#issuecomment-297333313","issue_url":"https://api.github.com/repos/tork-a/rtmros_nextage/issues/312","id":297333313,"user":{"login":"y-yosuke","id":3119480,"avatar_url":"https://avatars1.githubusercontent.com/u/3119480?v=3","gravatar_id":"","url":"https://api.github.com/users/y-yosuke","html_url":"https://github.com/y-yosuke","followers_url":"https://api.github.com/users/y-yosuke/followers","following_url":"https://api.github.com/users/y-yosuke/following{/other_user}","gists_url":"https://api.github.com/users/y-yosuke/gists{/gist_id}","starred_url":"https://api.github.com/users/y-yosuke/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/y-yosuke/subscriptions","organizations_url":"https://api.github.com/users/y-yosuke/orgs","repos_url":"https://api.github.com/users/y-yosuke/repos","events_url":"https://api.github.com/users/y-yosuke/events{/privacy}","received_events_url":"https://api.github.com/users/y-yosuke/received_events","type":"User","site_admin":false},"created_at":"2017-04-26T10:09:54Z","updated_at":"2017-04-26T10:09:54Z","body":"I wrote a `playPatternOfGroup()` example file `nextage_playpattern.py` as shown below.\r\n\r\n- newly defined functions\r\n  - circularPositions(center=[0.3, 0.2, 0.1], radius=0.1 ,steps=12)\r\n  - rectangularPositions(dp_a=[0.25, 0.0, 0.1], dp_b=[0.45, 0.2, 0.1])\r\n  - samePostureRPY(rpy, pat_length)\r\n  - equalTimeList(whole_tm, pat_length)\r\n  - setTargetPoseSequence(limb, pos_list, rpy_list, tm_list)\r\n\r\nI have confirmed the operation of this file in the simulation.\r\n\r\nWhen it seems that this file is good after some modifications,\r\nwould it be better to put the file in any extisting or new package?\r\nor I just paste it on the document of http://wiki.ros.org/rtmros_nextage/Tutorials/Task-oriented%20programming#Using_RTM_interface_commands ?\r\n\r\n```\r\n#!/usr/bin/env python\r\n# -*- coding: utf-8 -*-\r\n\r\n# Software License Agreement (BSD License)\r\n#\r\n# Copyright (c) 2017, Tokyo Opensource Robotics Kyokai Association\r\n# All rights reserved.\r\n#\r\n# Redistribution and use in source and binary forms, with or without\r\n# modification, are permitted provided that the following conditions\r\n# are met:\r\n#\r\n#  * Redistributions of source code must retain the above copyright\r\n#    notice, this list of conditions and the following disclaimer.\r\n#  * Redistributions in binary form must reproduce the above\r\n#    copyright notice, this list of conditions and the following\r\n#    disclaimer in the documentation and/or other materials provided\r\n#    with the distribution.\r\n#  * Neither the name of Tokyo Opensource Robotics Kyokai Association. nor the\r\n#    names of its contributors may be used to endorse or promote products\r\n#    derived from this software without specific prior written permission.\r\n#\r\n# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\r\n# \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\r\n# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\r\n# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\r\n# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\r\n# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\r\n# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\r\n# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\r\n# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\r\n# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\r\n# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\r\n# POSSIBILITY OF SUCH DAMAGE.\r\n#\r\n# Author: Isaac Isao Saito\r\n# Author: Yosuke Yamamoto\r\n\r\nimport socket\r\n\r\nfrom hironx_ros_bridge.ros_client import ROS_Client\r\n# This should come earlier than later import.\r\n# See http://code.google.com/p/rtm-ros-robotics/source/detail?r=6773\r\nfrom nextage_ros_bridge import nextage_client\r\nfrom rospy import ROSInitException\r\n\r\nfrom hrpsys import rtm\r\nimport argparse\r\n\r\nerrormsg_noros = 'No ROS Master found. Without it, you cannot use ROS from' \\\r\n                 ' this script, but can use RTM. To use ROS, do not forget' \\\r\n                 ' to run rosbridge. How to do so? --> http://wiki.ros.org/rtmros_nextage/Tutorials/Operating%20Hiro%2C%20NEXTAGE%20OPEN'\r\n\r\nimport math\r\n\r\n\r\ndef circularPositions(center=[0.3, 0.2, 0.1], radius=0.1 ,steps=12):\r\n    positions_xyz = []\r\n    step_rad = 2 * math.pi / steps\r\n    for i in range(steps+1):\r\n        ang_rad = step_rad * i\r\n        px = center[0] - radius * math.cos(ang_rad)\r\n        py = center[1] + radius * math.sin(ang_rad)\r\n        pz = center[2]\r\n        positions_xyz.append([px,py,pz])\r\n    \r\n    return positions_xyz\r\n\r\n\r\ndef rectangularPositions(dp_a=[0.25, 0.0, 0.1], dp_b=[0.45, 0.2, 0.1]):\r\n    positions_xyz = [\r\n                     [ dp_a[0], dp_a[1], dp_a[2] ],\r\n                     [ dp_a[0], dp_b[1], dp_a[2] ],\r\n                     [ dp_b[0], dp_b[1], dp_b[2] ],\r\n                     [ dp_b[0], dp_a[1], dp_b[2] ],\r\n                     [ dp_a[0], dp_a[1], dp_a[2] ]\r\n                    ]\r\n    \r\n    return positions_xyz\r\n    \r\n\r\ndef samePostureRPY(rpy, pat_length):\r\n    posture_rpy = []\r\n    for i in range(pat_length):\r\n        posture_rpy.append(rpy)\r\n    \r\n    return posture_rpy\r\n\r\n\r\ndef equalTimeList(whole_tm, pat_length):\r\n    tm_list = []\r\n    tm = whole_tm / pat_length\r\n    for i in range(pat_length):\r\n        tm_list.append(tm)\r\n    \r\n    return tm_list\r\n\r\n\r\ndef setTargetPoseSequence(limb, pos_list, rpy_list, tm_list):\r\n    pattern_arm = []\r\n    for pos, rpy, tm in zip(pos_list, rpy_list, tm_list):\r\n        robot.setTargetPose(limb, pos, rpy, tm)\r\n        robot.waitInterpolationOfGroup(limb)\r\n        if limb == 'rarm':\r\n            joint_angles_deg = robot.getJointAngles()[3:9]\r\n        else:\r\n            joint_angles_deg = robot.getJointAngles()[9:]\r\n        joint_angles_rad = [math.radians(angle_in_degree) for angle_in_degree in joint_angles_deg]\r\n        pattern_arm.append(joint_angles_rad)\r\n\r\n    return pattern_arm\r\n\r\n\r\nif __name__ == '__main__':\r\n    parser = argparse.ArgumentParser(description='hiro command line interpreters')\r\n    parser.add_argument('--host', help='corba name server hostname')\r\n    parser.add_argument('--port', help='corba name server port number')\r\n    parser.add_argument('--modelfile', help='robot model file nmae')\r\n    parser.add_argument('--robot', help='robot modlule name (RobotHardware0 for real robot, Robot()')\r\n    parser.add_argument('--dio_ver', help=\"Version of digital I/O. Only users \"\r\n                        \"whose robot was shipped before Aug 2014 need to \"\r\n                        \"define this, and the value should be '0.4.2'.\")\r\n    args, unknown = parser.parse_known_args()\r\n\r\n    if args.host:\r\n        rtm.nshost = args.host\r\n    if args.port:\r\n        rtm.nsport = args.port\r\n    if not args.robot:\r\n        args.robot = \"RobotHardware0\" if args.host else \"HiroNX(Robot)0\"\r\n    if not args.modelfile:\r\n        args.modelfile = \"/opt/jsk/etc/NEXTAGE/model/main.wrl\" if args.host else \"\" \r\n\r\n    # support old style format\r\n    if len(unknown) >= 2:\r\n        args.robot = unknown[0]\r\n        args.modelfile = unknown[1]\r\n    robot = nxc = nextage_client.NextageClient()\r\n    # Use generic name for the robot instance. This enables users on the\r\n    # script commandline (eg. ipython) to run the same commands without asking\r\n    # them to specifically tell what robot they're using (eg. hiro, nxc).\r\n    # This is backward compatible so that users can still keep using `nxc`.\r\n    # See http://code.google.com/p/rtm-ros-robotics/source/detail?r=6926\r\n    robot.init(robotname=args.robot, url=args.modelfile)\r\n\r\n    if args.dio_ver:\r\n        robot.set_hand_version(args.dio_ver)\r\n\r\n    try:\r\n        ros = ROS_Client()\r\n    except ROSInitException as e:\r\n        print('[nextage.py] {}'.format(e))\r\n    except socket.error as e: \r\n        print(\"\\033[31m%s\\n%s\\033[0m\" % (e.strerror, errormsg_noros))\r\n    \r\n    \r\n    # playPatternOfGroup() Example\r\n    robot.goInitial()\r\n\r\n    positions_arm = []  # All elements are lists. E.g. pos1 = [x1, y1, z1]\r\n    rpys_arm = []       # All elements are lists. E.g. rpy1 = [r1, p1, y1]\r\n    time_list = []      # Time list [t1, t2, t3,...]\r\n    limb_name = 'larm'  # Limb 'rarm', 'larm'\r\n    \r\n    # Rectangular Positions, RPYs, Time List\r\n    rect_xyzs = rectangularPositions(dp_a=[0.25, 0.0, 0.1], dp_b=[0.45, 0.2, 0.1])\r\n    rect_rpys = samePostureRPY([-3.073437, -1.569023, 3.073247], len(rect_xyzs))\r\n    rect_time = equalTimeList(10.0, len(rect_xyzs))\r\n    \r\n    # Circular Positions, RPYs, Time List\r\n    circ_xyzs = circularPositions(center=[0.35, 0.1, 0.1], radius=0.1 ,steps=12)\r\n    circ_rpys = samePostureRPY([-3.073437, -1.569023, 3.073247], len(circ_xyzs))\r\n    circ_time = equalTimeList(10.0, len(circ_xyzs))\r\n\r\n    # Adjust Transition Time\r\n    rect_time[0] += 1.0\r\n    circ_time[0] += 1.0\r\n    \r\n    # Combine Patterns\r\n    positions_arm = rect_xyzs + circ_xyzs\r\n    rpys_arm      = rect_rpys + circ_rpys\r\n    time_list     = rect_time + circ_time\r\n\r\n    print('Limb: %s' % limb_name)\r\n    print('Positions (%d): %s' % (len(positions_arm), positions_arm))\r\n    print('RPY Pattern (%d): %s' % (len(rpys_arm), rpys_arm))\r\n    print('Time List (%d): %s' % (len(time_list), time_list))\r\n    \r\n    # Generate Joint Angle Pattern with Moving\r\n    print('Generating Joint Angle Pattern')\r\n    play_pattern_arm = setTargetPoseSequence(limb_name, positions_arm, rpys_arm, time_list)\r\n    play_pattern_time = time_list\r\n    \r\n    print('Generated')\r\n    print('Limb: %s' % limb_name)\r\n    print('Joint Angle Pattern [rad]: %s' % play_pattern_arm)\r\n    print('Motion Time Pattern [sec]: %s' % play_pattern_time)\r\n\r\n    robot.goInitial()\r\n    \r\n    # Play Pattern - playPatternOfGroup()\r\n    print('Start: Play Pattern')\r\n    robot.playPatternOfGroup(limb_name, play_pattern_arm, play_pattern_time)\r\n    robot.waitInterpolationOfGroup(limb_name)\r\n    print('End: Play Pattern')\r\n\r\n    robot.goInitial()\r\n\r\n# for simulated robot\r\n# $ ./hironx.py\r\n#\r\n# for real robot\r\n# ./hironx.py  --host hiro014\r\n# ./ipython -i hironx.py --host hiro014\r\n# for real robot with custom model file\r\n# ./hironx.py  --host hiro014 --modelfile /opt/jsk/etc/NEXTAGE/model/main.wrl\r\n```"}}
{"actor":{"display_login":"y-yosuke"}}