'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'],
'left_arm': ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint',
'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint']
}
arm_name = 'right_arm'
joint_positions = [
[-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338],
[0.002, 0.076, -1.146, -1.255, 61.306, -0.526, 3.299],
[0.681, 0.720, 0.147, -0.540, 63.062, -0.013, 2.245]
]
rospy.init_node('test_arm_control', anonymous=True)
ac = ArmController(arm_name)
ii = 0
while True:
js = JointState()
js.name = groups[arm_name]
js.position = joint_positions[ii]
try:
print 'Moving to %s' % (str(joint_positions[ii]))
ac.move_to_joint_state(js,
joint_bounds=None, try_hard=True, planner_timeout=5., ordered_collisions=None, planner_id='')
except ArmNavError as e:
print 'Arm nav error: %s' % str(e)
ii = (ii + 1) % len(joint_positions)
rospy.sleep(0.1)
#
# Author: Jon Binney
import roslib; roslib.load_manifest('pr2_python')
import numpy as np
import rospy
from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped
from tf import transformations
from pr2_python.arm_controller import ArmController
from pr2_python.exceptions import ArmNavError
arm_name = 'right_arm'
rospy.init_node('test_arm_control', anonymous=True)
ac = ArmController(arm_name)
while not rospy.is_shutdown():
rot = transformations.random_quaternion()
#trans = np.random.uniform(-0.5, 0.5, 3) + np.array((
trans = np.array((0.65, 0.0, 0.0)) + np.random.uniform(-0.6, 0.6, 3)
rot = np.array((0.0, 0.0, 0.0, 1.0))
print 'Moving to %s, %s' % (str(rot), str(trans))
goal_pose = PoseStamped()
goal_pose.pose.position = Point(*trans)
goal_pose.pose.orientation = Quaternion(*rot)
goal_pose.header.frame_id = '/torso_lift_link'
try:
ac.move_to_goal(goal_pose, try_hard=True)
print ac.get_exceptions()
except ArmNavError as e:
