Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add test for joint trajectory action cancel and overwrite #1070

Merged
merged 6 commits into from
Dec 18, 2019
Merged

Add test for joint trajectory action cancel and overwrite #1070

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
d80b684
Add test for joint trajectory action cancel with PA10
pazeshun Oct 25, 2019
dd471ee
Add test for joint trajectory action goal overwrite with PA10
pazeshun Oct 25, 2019
5a82afb
Add test for joint trajectory action cancel overwrite with samplerobot
pazeshun Oct 25, 2019
1877af2
Add two points trajectory to PA10 test of joint trajectory action goa…
pazeshun Nov 30, 2019
33f48f9
Add comment on two points trajectory to samplerobot test of joint tra…
pazeshun Nov 30, 2019
d51fcac
Add two points trajectory to test of joint trajectory action cancel
pazeshun Nov 30, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
171 changes: 171 additions & 0 deletions hrpsys_ros_bridge/test/test-pa10.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,177 @@ def test_joint_states_velocity(self):
rospy.logwarn("difference from predicted velocity: %r"%(linalg.norm(array(real_vel)-array(pred_vel))))
self.assertAlmostEqual(linalg.norm(array(real_vel)-array(pred_vel)), 0, delta=0.1)

# https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-120208947
def test_jta_cancel_goal(self):
# for < kinetic
if os.environ['ROS_DISTRO'] >= 'kinetic' :
return True
from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
larm = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction)
self.impl_test_jta_cancel_goal(larm, JointTrajectoryGoal())

def test_fjta_cancel_goal(self):
# for >= kinetic
if os.environ['ROS_DISTRO'] < 'kinetic' :
return True
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
larm = actionlib.SimpleActionClient("/fullbody_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
self.impl_test_jta_cancel_goal(larm, FollowJointTrajectoryGoal())

def impl_test_jta_cancel_goal(self, larm, goal):
larm.wait_for_server()

try:
self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120))
except tf.Exception:
self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK")
goal.trajectory.joint_names = ["J1","J2","J3","J4","J5","J6","J7"]

point1 = JointTrajectoryPoint()
point2 = JointTrajectoryPoint()
# cancel one point trajectory (:angle-vector)
point1.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
point1.time_from_start = rospy.Duration(5.0)
self.jta_cancel_goal_template(larm, goal, [point1])

# cancel two points trajectory (:angle-vector-sequence)
point1.positions = [ x * math.pi / 180.0 for x in [5,10,15,20,25,30,35] ]
point1.time_from_start = rospy.Duration(2.5)
point2.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
point2.time_from_start = rospy.Duration(5.0)
self.jta_cancel_goal_template(larm, goal, [point1, point2])

def jta_cancel_goal_template(self, larm, goal, points):
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.points = points
larm.send_goal(goal)
rospy.sleep(2.5)
(trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans1,rot1))
larm.cancel_goal()
rospy.sleep(2.5)
(trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
larm.wait_for_result()
rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)

# https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-392741195
def test_jta_overwrite_goal(self):
# for < kinetic
if os.environ['ROS_DISTRO'] >= 'kinetic' :
return True
from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
larm = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction)
self.impl_test_jta_overwrite_goal(larm, JointTrajectoryGoal())

def test_fjta_overwrite_goal(self):
# for >= kinetic
if os.environ['ROS_DISTRO'] < 'kinetic' :
return True
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
larm = actionlib.SimpleActionClient("/fullbody_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
self.impl_test_jta_overwrite_goal(larm, FollowJointTrajectoryGoal())

def impl_test_jta_overwrite_goal(self, larm, goal):
larm.wait_for_server()

try:
self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120))
except tf.Exception:
self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK")
(trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans1,rot1))
goal.trajectory.joint_names = ["J1","J2","J3","J4","J5","J6","J7"]

point1 = JointTrajectoryPoint()
point2 = JointTrajectoryPoint()
point3 = JointTrajectoryPoint()
point4 = JointTrajectoryPoint()
# one point trajectory (:angle-vector) -> one point trajectory (:angle-vector)
rospy.logwarn("one point trajectory -> one point trajectory")
## first point
point1.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
point1.time_from_start = rospy.Duration(5.0)
## second point
point2.positions = [0,0,0,0,0,0,0]
point2.time_from_start = rospy.Duration(2.5)
## execution
self.jta_overwrite_goal_template(larm, goal, trans1, [point1], [point2])

# one point trajectory (:angle-vector) -> two points trajectory (:angle-vector-sequence)
rospy.logwarn("one point trajectory -> two points trajectory")
## first point
point1.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
point1.time_from_start = rospy.Duration(5.0)
## second point
point2.positions = [ x * math.pi / 180.0 for x in [2.5,5,7.5,10,12.5,15,17.5] ]
point2.time_from_start = rospy.Duration(1.25)
## third point
point3.positions = [0,0,0,0,0,0,0]
point3.time_from_start = rospy.Duration(2.5)
## execution
self.jta_overwrite_goal_template(larm, goal, trans1, [point1], [point2, point3])

# two points trajectory (:angle-vector-sequence) -> one point trajectory (:angle-vector)
rospy.logwarn("two points trajectory -> one point trajectory")
## first point
point1.positions = [ x * math.pi / 180.0 for x in [5,10,15,20,25,30,35] ]
point1.time_from_start = rospy.Duration(2.5)
## second point
point2.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
point2.time_from_start = rospy.Duration(5.0)
## third point
point3.positions = [0,0,0,0,0,0,0]
point3.time_from_start = rospy.Duration(2.5)
## execution
self.jta_overwrite_goal_template(larm, goal, trans1, [point1, point2], [point3])

# two points trajectory (:angle-vector-sequence) -> two points trajectory (:angle-vector-sequence)
rospy.logwarn("two points trajectory -> two points trajectory")
## first point
point1.positions = [ x * math.pi / 180.0 for x in [5,10,15,20,25,30,35] ]
point1.time_from_start = rospy.Duration(2.5)
## second point
point2.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
point2.time_from_start = rospy.Duration(5.0)
## third point
point3.positions = [ x * math.pi / 180.0 for x in [2.5,5,7.5,10,12.5,15,17.5] ]
point3.time_from_start = rospy.Duration(1.25)
## fourth point
point4.positions = [0,0,0,0,0,0,0]
point4.time_from_start = rospy.Duration(2.5)
## execution
self.jta_overwrite_goal_template(larm, goal, trans1, [point1, point2], [point3, point4])

def jta_overwrite_goal_template(self, larm, goal, trans1, points1, points2):
# send first goal
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.points = points1
larm.send_goal(goal)
rospy.sleep(2.5)
# send second goal
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.points = points2
larm.send_goal(goal)
# check robot motion after it is overwritten
jt_pos1 = rospy.wait_for_message('/joint_states', JointState)
rospy.logwarn("joint position just after sending second goal: %r"%(array(jt_pos1.position)))
rospy.sleep(0.2)
jt_pos2 = rospy.wait_for_message('/joint_states', JointState)
rospy.logwarn("joint position 0.2 sec after sending second goal: %r"%(array(jt_pos2.position)))
rospy.sleep(2.3)
(trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
larm.wait_for_result()
rospy.logwarn("difference between two joint positions %r"%(array(jt_pos2.position)-array(jt_pos1.position)))
## if robot suddenly stops when goal is overwritten, joint position immediately starts to decrease (heading for new goal).
## joint_states includes unchanged hand joints, so using assertGreaterEqual instead of assertGreater is necessary.
for x in array(jt_pos2.position)-array(jt_pos1.position):
self.assertGreaterEqual(x, 0)
rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)


# unittest.main()
if __name__ == '__main__':
Expand Down
190 changes: 190 additions & 0 deletions hrpsys_ros_bridge/test/test-samplerobot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -248,6 +248,196 @@ def test_joint_angles_duration_0(self): # https://github.com/start-jsk/rtmros_co
rospy.logwarn("difference between two angles %r %r"%(array([30,30,30,-90,-40,-30])-array(goal_angles),linalg.norm(array([30,30,30,-90,-40,-30])-array(goal_angles))))
self.assertAlmostEqual(linalg.norm(array([30,30,30,-90,-40,-30])-array(goal_angles)), 0, delta=0.1)

# https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-120208947
def test_jta_cancel_goal(self):
# for < kinetic
if os.environ['ROS_DISTRO'] >= 'kinetic' :
return True
from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
self.impl_test_jta_cancel_goal(larm, JointTrajectoryGoal())

def test_fjta_cancel_goal(self):
# for >= kinetic
if os.environ['ROS_DISTRO'] < 'kinetic' :
return True
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
larm = actionlib.SimpleActionClient("/larm_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
self.impl_test_jta_cancel_goal(larm, FollowJointTrajectoryGoal())

def impl_test_jta_cancel_goal(self, larm, goal):
larm.wait_for_server()

# initialize
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.joint_names = ["LARM_SHOULDER_P","LARM_SHOULDER_R","LARM_SHOULDER_Y","LARM_ELBOW","LARM_WRIST_Y","LARM_WRIST_P"]
point = JointTrajectoryPoint()
point.positions = [0,0,0,0,0,0]
point.time_from_start = rospy.Duration(5.0)
goal.trajectory.points.append(point)
larm.send_goal(goal)
larm.wait_for_result()

try:
self.listener.waitForTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(), rospy.Duration(120))
except tf.Exception:
self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /LARM_LINK7")

point1 = JointTrajectoryPoint()
point2 = JointTrajectoryPoint()
# cancel one point trajectory (:angle-vector)
point1.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
point1.time_from_start = rospy.Duration(5.0)
self.jta_cancel_goal_template(larm, goal, [point1])

# cancel two points trajectory (:angle-vector-sequence)
point1.positions = [ x * math.pi / 180.0 for x in [10,10,10,10,10,10] ]
point1.time_from_start = rospy.Duration(2.5)
point2.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
point2.time_from_start = rospy.Duration(5.0)
self.jta_cancel_goal_template(larm, goal, [point1, point2])

def jta_cancel_goal_template(self, larm, goal, points):
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.points = points
larm.send_goal(goal)
rospy.sleep(2.5)
(trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1))
larm.cancel_goal()
rospy.sleep(2.5)
(trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
larm.wait_for_result()
rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)

# https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-392741195
def test_jta_overwrite_goal(self):
# for < kinetic
if os.environ['ROS_DISTRO'] >= 'kinetic' :
return True
from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
self.impl_test_jta_overwrite_goal(larm, JointTrajectoryGoal())

def test_fjta_overwrite_goal(self):
# for >= kinetic
if os.environ['ROS_DISTRO'] < 'kinetic' :
return True
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
larm = actionlib.SimpleActionClient("/larm_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
self.impl_test_jta_overwrite_goal(larm, FollowJointTrajectoryGoal())

def impl_test_jta_overwrite_goal(self, larm, goal):
larm.wait_for_server()

# initialize
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.joint_names = ["LARM_SHOULDER_P","LARM_SHOULDER_R","LARM_SHOULDER_Y","LARM_ELBOW","LARM_WRIST_Y","LARM_WRIST_P"]
point = JointTrajectoryPoint()
point.positions = [0,0,0,0,0,0]
point.time_from_start = rospy.Duration(5.0)
goal.trajectory.points = [point]
larm.send_goal(goal)
larm.wait_for_result()

try:
self.listener.waitForTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(), rospy.Duration(120))
except tf.Exception:
self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /LARM_LINK7")
(trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1))

point1 = JointTrajectoryPoint()
point2 = JointTrajectoryPoint()
point3 = JointTrajectoryPoint()
point4 = JointTrajectoryPoint()
# one point trajectory (:angle-vector) -> one point trajectory (:angle-vector)
rospy.logwarn("one point trajectory -> one point trajectory")
## first point
point1.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
point1.time_from_start = rospy.Duration(5.0)
## second point
point2.positions = [0,0,0,0,0,0]
point2.time_from_start = rospy.Duration(2.5)
## execution
self.jta_overwrite_goal_template(larm, goal, trans1, [point1], [point2])

# hrpsys 315.16.0 (having https://github.com/fkanehiro/hrpsys-base/pull/1237) is needed for the following tests to pass
# # one point trajectory (:angle-vector) -> two points trajectory (:angle-vector-sequence)
# rospy.logwarn("one point trajectory -> two points trajectory")
# ## first point
# point1.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
# point1.time_from_start = rospy.Duration(5.0)
# ## second point
# point2.positions = [ x * math.pi / 180.0 for x in [5,5,5,5,5,5] ]
# point2.time_from_start = rospy.Duration(1.25)
# ## third point
# point3.positions = [0,0,0,0,0,0]
# point3.time_from_start = rospy.Duration(2.5)
# ## execution
# self.jta_overwrite_goal_template(larm, goal, trans1, [point1], [point2, point3])

# # two points trajectory (:angle-vector-sequence) -> one point trajectory (:angle-vector)
# rospy.logwarn("two points trajectory -> one point trajectory")
# ## first point
# point1.positions = [ x * math.pi / 180.0 for x in [10,10,10,10,10,10] ]
# point1.time_from_start = rospy.Duration(2.5)
# ## second point
# point2.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
# point2.time_from_start = rospy.Duration(5.0)
# ## third point
# point3.positions = [0,0,0,0,0,0]
# point3.time_from_start = rospy.Duration(2.5)
# ## execution
# self.jta_overwrite_goal_template(larm, goal, trans1, [point1, point2], [point3])

# # two points trajectory (:angle-vector-sequence) -> two points trajectory (:angle-vector-sequence)
# rospy.logwarn("two points trajectory -> two points trajectory")
# ## first point
# point1.positions = [ x * math.pi / 180.0 for x in [10,10,10,10,10,10] ]
# point1.time_from_start = rospy.Duration(2.5)
# ## second point
# point2.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
# point2.time_from_start = rospy.Duration(5.0)
# ## third point
# point3.positions = [ x * math.pi / 180.0 for x in [5,5,5,5,5,5] ]
# point3.time_from_start = rospy.Duration(1.25)
# ## fourth point
# point4.positions = [0,0,0,0,0,0]
# point4.time_from_start = rospy.Duration(2.5)
# ## execution
# self.jta_overwrite_goal_template(larm, goal, trans1, [point1, point2], [point3, point4])

def jta_overwrite_goal_template(self, larm, goal, trans1, points1, points2):
# send first goal
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.points = points1
larm.send_goal(goal)
rospy.sleep(2.5)
# send second goal
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.points = points2
larm.send_goal(goal)
# check robot motion after it is overwritten
jt_pos1 = rospy.wait_for_message('/joint_states', JointState)
rospy.logwarn("joint position just after sending second goal: %r"%(array(jt_pos1.position)))
rospy.sleep(0.2)
jt_pos2 = rospy.wait_for_message('/joint_states', JointState)
rospy.logwarn("joint position 0.2 sec after sending second goal: %r"%(array(jt_pos2.position)))
rospy.sleep(2.3)
(trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
larm.wait_for_result()
rospy.logwarn("difference between two joint positions %r"%(array(jt_pos2.position)-array(jt_pos1.position)))
## if robot suddenly stops when goal is overwritten, joint position immediately starts to decrease (heading for new goal).
## joint_states includes unchanged hand joints, so using assertGreaterEqual instead of assertGreater is necessary.
for x in array(jt_pos2.position)-array(jt_pos1.position):
self.assertGreaterEqual(x, 0)
rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)


#unittest.main()
if __name__ == '__main__':
Expand Down