use setJointAnglesOfGroup() and playPattern(), use clearJointAngles()

[k-okada]

this will enable cancelGoal of joint trajectory action server

[CAUTION!!!] this will use different API for limb controller, so we designed new API to have same functionality as old one, but it may change the behavior if there are bug.

[k-okada]

to : real robot owners : please check this PR before we merge.

[snozawa]

How can we test this?

(send *robot* :reset-maip-pose)
(send *ri* :angle-vector (send *robot* :angle-vector) 5000)
(read-line)
(send *ri* :cancel-angle-vector)

[k-okada]

that should work

on python

        larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
        larm.wait_for_server()

        goal = JointTrajectoryGoal()
        goal.trajectory.header.stamp = rospy.get_rostime()
        goal.trajectory.joint_names.append("LARM_SHOULDER_P")
        goal.trajectory.joint_names.append("LARM_SHOULDER_R")
        goal.trajectory.joint_names.append("LARM_SHOULDER_Y")
        goal.trajectory.joint_names.append("LARM_ELBOW")
        goal.trajectory.joint_names.append("LARM_WRIST_Y")
        goal.trajectory.joint_names.append("LARM_WRIST_P")

        point = JointTrajectoryPoint()
        point.positions = [ x * math.pi / 180.0 for x in [30,30,30,-90,-40,-30] ]
        point.time_from_start = rospy.Duration(5.0)
        goal.trajectory.points.append(point)
        point = JointTrajectoryPoint()
        point.positions = [ x * math.pi / 180.0 for x in [0,0,0,0,0,0] ]
        point.time_from_start = rospy.Duration(10.0)
        goal.trajectory.points.append(point)
        print goal.trajectory.points
        larm.send_goal(goal)
        #larm.wait_for_result()                                                                                             
        time.sleep(3)
        larm.cancel_goal()                       

[k-okada]

これ，どうでしょうか？これがないとangle-vector-sequenceで送った後にcancelできないので，テレオペ等では有効だと思っているんですが．．．

[k-okada]

cc @terasawa

[130s]

As I reported in tork-a/rtmros_nextage#284 (comment) I confirmed that cancel command works with NEXTAGE on simulator. If any of Hironx/NEXTAGE Open users are interested in testing this on the real robot we're willing to help you.

[130s]

I assume this doesn't require higher version of hrpsys (than the ones commonly used for NEXTAGE, 315.2.8 or to the least 315.1.9), looking at the functions in use, i.e. setJointAnglesSequence, clearJointAngles.

UPDATE: This PR does require higher version. See #765 (comment) and the following conversation.

[k-okada]

https://github.com/fkanehiro/hrpsys-base/blob/315.1.9/idl/SequencePlayerService.idl https://github.com/fkanehiro/hrpsys-base/blob/315.2.8/idl/SequencePlayerService.idl https://github.com/fkanehiro/hrpsys-base/blob/master/idl/SequencePlayerService.idl I do not think we have clearJointAngles function in old hrpsys, 1) need to update idl files to include information on version we can use new featrue 2) it looks like we have clear/ clearNoWait fuction in old hrpsys, so may be we can use that. need to look into more deeply.btw do we have test code which checks this feature?

…

-- ◉ Kei Okada 2017-01-27 2:50 GMT+09:00 Isaac I.Y. Saito <[email protected]>:

I assume this doesn't require higher version of hrpsys (than the ones commonly used for NEXTAGE, 315.2.8 or to the least 315.1.9), looking at the functions in use, i.e. setJointAnglesSequence, clearJointAngles. — You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub <#765 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AAeG3FZF_TNPUD6HQ3FwBMsgRb99NrGKks5rWNzzgaJpZM4FVwgZ> .

[130s]

Sorry, looks like my comment #765 (comment) was wrong. clearJointAngles was added to the upstream, hrpsys, in fkanehiro/hrpsys-base#665 and setJointAnglesSequence in fkanehiro/hrpsys-base#573. Both became available since 315.5.0 presumably.

[7675t]

こちら，Kineticでビルドして試しましたが，軌道がおかしいのは直りましたが，動作のストップは効かないようです．hrpsysの方も fkanehiro/hrpsys-base#1237 が必要ということでしょうか？ Kineticのhrpsysは以下です．

ii  ros-kinetic-hrpsys                               315.14.0-0xenial-20171104-182634-0800                 amd64        An OpenRTM-aist-based robot controller.

[k-okada]

You may not need fkanehiro/hrpsys-base#1237 , because a489f5f use old api for hrpsys < 315.16.0. Please check "Hrpsys viewer", because planned motion in rviz does not stop, when you push "stop" button, but that signal will send to hrpsys and stop execution

please also confirm if you can see folloinwg message, make sure that ROS bridge send 'clearOfGroup: rarm '.

[ INFO] [1518498649.682171977, 172.679999999]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 196[Hz] (exec 1261 Hz, dropped 196)
[ INFO] [1518498650.573884624, 173.529999999]: [HrpsysJointTrajectoryBridge0] @onFollowJointTrajectoryActionPreempt()
[ INFO] [1518498650.574057661, 173.529999999]: [HrpsysJointTrajectoryBridge0] clearOfGroup: rarm

[7675t]

Sorry, this was my fault, the workspace setting was wrong.
I confirmed this PR works correctly with released packages in simulation, both with NXO and HIRO.
Thank you.

[aislab]

We tried this on real robot Hironx.
It was confirmed that it works correctly.

We tried it on ROS Kinetic.

[7675t]

Great, thanks! @aislab @k-okada

[pazeshun]

@k-okada
With this PR, (send *ri* :cancel-angle-vector) works with Hironx in JSK.

The behavior when a new command comes to follow joint trajectory action before the previous command is completed is also changed.
Before this PR, motion following the previous command is smoothly changed to follow the new command.
The previous command is not completed.
With this PR, motion following the previous command is immediately stopped and changed to follow the new command.
This behavior is not desirable, I think.
http://wiki.ros.org/robot_mechanism_controllers/JointTrajectoryActionController

[Affonso-Gui]

@k-okada After including k-okada#10, can we merge this into master? Have been using in Hiro without problems.

[pazeshun]

With this PR, motion following the previous command is immediately stopped and changed to follow the new command.

I found that this behaviour is due to clear(OfGroup) (old hrpsys).
When clearJointAngles(OfGroup) is used (new hrpsys), the robot smoothly stops.
This means HRP2 smoothly changes its motion after new command comes.

[pazeshun]

The contents of this PR were merged to master via #1071