def __init__(self):
self._joint_state_listener = JointStateListener()
self._arm_planners = {}
self.side_joint_trajectory = {}
self._arm_mover = ArmMover()
for arm in ['left_arm', 'right_arm']:
self._arm_planners[arm] = ArmPlanner(arm)
self.side_joint_trajectory[arm] = rospy.get_param('/arm_configurations/side_tuck/trajectory/'+arm,
DEFAULT_SIDE_JOINT_TRAJECTORY[arm])
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Jon Binney
import roslib;
roslib.load_manifest('pr2_plan_utils')
import rospy
from geometry_msgs.msg import PoseStamped
from pr2_plan_utils.arm_mover import ArmMover
arm_name = 'right_arm'
# initialize
rospy.init_node('test_arm_control', anonymous=True)
arm_mover = ArmMover()
# generate desired arm pose
goal_pose = PoseStamped()
goal_pose.pose.position.x = 0.65
goal_pose.pose.orientation.w = 1.0
goal_pose.header.frame_id = '/torso_lift_link'
handle = arm_mover.move_to_goal(arm_name, goal_pose)
if handle.reached_goal():
print 'Reached the goal!'
else:
print handle.get_errors()
class ArmTasks:
'''
Performs high level tasks for the PR2 arms.
This class does high level tasks for the arms alone.
**Attributes:**
**side_joint_trajectory ([[double]]):** Joint trajectory for moving to the side
'''
def __init__(self):
self._joint_state_listener = JointStateListener()
self._arm_planners = {}
self.side_joint_trajectory = {}
self._arm_mover = ArmMover()
for arm in ['left_arm', 'right_arm']:
self._arm_planners[arm] = ArmPlanner(arm)
self.side_joint_trajectory[arm] = rospy.get_param('/arm_configurations/side_tuck/trajectory/'+arm,
DEFAULT_SIDE_JOINT_TRAJECTORY[arm])
def move_arm_to_side(self, arm_name):
'''
A function that persistently tries to move an arm to the side.
Unless the arm is physically stuck in the world, the arm will be at the robot's side at the end of this
function. However, as last resort, the robot uses an open-loop movement to a specific joint state so there
is no guarantee the arm's path will be collision-free.
Specifically, this function tries in order to:
1. Move the arm to the side joint pose using collision-free planning (calls move arm)
2. If the starting state is in collision, it tries twice to move the state out of collision by
executing a short open-loop trajectory to a free state.
3. If the starting state is outside joint angles, it tries twice to move the state into joint angles by
executing a short open-loop trajectory to a state within joint angle limits.
4. Moves open loop to each of the joint states along the side joint trajectory. By defalut there are
three of these points. The first is high, near the center of the robot's body. The second is
high and to the robot's side. The last is low and at the robots side. After each of the first two
moves, the robot tries to move to the final joint state collision free before executing the next
trajectory open-loop.
**Args:**
**arm_name (string):** The name of the arm ('right_arm' or 'left_arm') to be moved to side.
'''
if self._at_side(arm_name):
rospy.loginfo(arm_name + ' is already at the side position')
return
self._arm_mover.move_into_joint_limits(arm_name)
joint_state = self._arm_planners[arm_name].joint_positions_to_joint_state(
self.side_joint_trajectory[arm_name][-1])
# first try to move directly there using move arm
handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0)
if handle.reached_goal():
return
for t in range(2):
checked_side = False
for e in handle.get_errors():
rospy.loginfo('When moving arm, error was:' + str(e))
if type(e) != ArmNavError or not e.error_code:
if not checked_side and self._at_side(arm_name):
return
checked_side = True
elif e.error_code.val == e.error_code.START_STATE_IN_COLLISION:
self._arm_mover.move_out_of_collision(arm_name)
handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0)
if handle.reached_goal():
return
rospy.loginfo('Even after moving out of collision, we are unable to move the arm to the side')
break
elif e.error_code.val == e.error_code.JOINT_LIMITS_VIOLATED:
self._arm_mover.move_into_joint_limits(arm_name)
handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0)
if handle.reached_goal():
return
rospy.loginfo('Moved into joint limits but still unable to move the arm to the side')
break
# now try to move to each point on our open loop trajectories
# when there, see if we can move to side
for p in range(len(self.side_joint_trajectory[arm_name])):
joint_state.position = self.side_joint_trajectory[arm_name][p]
self._arm_mover.move_to_goal(arm_name, joint_state, try_hard=True)
if p < len(self.side_joint_trajectory[arm_name]) - 1:
# try to go directly to side from here
joint_state.position = self.side_joint_trajectory[arm_name][-1]
handle = self._arm_mover.move_to_goal_using_move_arm(arm_name, joint_state, 10.0)
if handle.reached_goal() or self._at_side(arm_name):
return
def move_arms_to_side(self):
'''
Moves both arms to side by calling move_arm_to_side on each arm.
'''
self.move_arm_to_side('right_arm')
self.move_arm_to_side('left_arm')
def _at_side(self, arm_name):
# check if we're already pretty close to the side position
# move arm only does this if we're not in collision (which is kind of dumb)
joint_state = self._arm_planners[arm_name].joint_positions_to_joint_state(
self.side_joint_trajectory[arm_name][-1])
current_joint_state = self._joint_state_listener.get_joint_positions(self._arm_planners[arm_name].joint_names)
already_there = True
for p in range(len(current_joint_state)):
if abs(gt.angular_distance(current_joint_state[p], joint_state.position[p])) > 0.2:
already_there = False
rospy.loginfo('Joint '+joint_state.name[p]+' is out of position by '
+ str(gt.angular_distance(current_joint_state[p], joint_state.position[p])))
break
return already_there
def __init__(self, name):
self._arm_mover = ArmMover()
#self._tasks = Tasks()
self._arm_tasks = ArmTasks()
self._base = base.Base()
self._head = head.Head()
self._torso = torso.Torso()
self._tf = tf.TransformListener( True, rospy.Duration(100) )
self._tfBr = tf.TransformBroadcaster()
#################### MOVE TORSO TO MAX HEIGHT ####################
self._torso.up()
rospy.loginfo('[gtp] Torso lifted!')
#################### TUCK LEFT ARM #######################
#self._tasks.move_arms_to_side()
self._arm_tasks.move_arm_to_side('left_arm')
rospy.loginfo('[gtp] Left arm moved to the side!')
#action_name = 'tuck_arms'
#tuck_arms_action_server = tuck_arms_main.TuckArmsActionServer(action_name)
#tuck_arm_client = actionlib.SimpleActionClient(action_name, TuckArmsAction)
#rospy.logdebug('Waiting for tuckarm action server to start')
#tuck_arm_client.wait_for_server(rospy.Duration(10.0))
#rospy.logdebug('Tucking left arm...')
#goal = TuckArmsGoal()
#goal.tuck_left = True
#goal.tuck_right = False
#tuck_arm_client.send_goal_and_wait(goal, rospy.Duration(30.0), rospy.Duration(5.0))
#################### MOVE RIGHT ARM TO INITAL POSE #######################
#self._arm_tasks.move_arm_to_side('right_arm')
#rospy.loginfo('[gtp] Right arm moved to the side!')
self._init_arm_pose = PoseStamped()
self._init_arm_pose.pose.position.x = 0.282
self._init_arm_pose.pose.position.y = -0.485
self._init_arm_pose.pose.position.z = 0.303
self._init_arm_pose.pose.orientation.x = 0.030
self._init_arm_pose.pose.orientation.y = 0.342
self._init_arm_pose.pose.orientation.z = -0.057
self._init_arm_pose.pose.orientation.w = 0.937
self._init_arm_pose.header.frame_id = 'torso_lift_link'
for t in range(0,3):
handle = self._arm_mover.move_to_goal( 'right_arm', self._init_arm_pose,
collision_aware_goal=True, planner_timeout=10.,
bounds=(0.01, 0.01, 0.01, 0.1, 0.1, 0.1),
try_hard=False )
if handle.reached_goal():
break
if handle.reached_goal():
rospy.loginfo('[gtp] Right arm init pose reached!')
else:
rospy.logerr('[gtp] Moving right arm to init pose failed!')
rospy.logerr( handle.get_errors() )
#################### STRAIGHTEN HEAD #################
self._head.look_at_relative_point(0.8, 0.0, 0.5)
rospy.loginfo('[gtp] Head straightened!')
rospy.loginfo('[gtp] Attempting to reach initial pose...')
init_goal = self._get_init_goal()
success = self._go_to_pose( init_goal )
if success:
rospy.loginfo('[gtp] Initial pose successfully reached!')
else:
rospy.logwarn('[gtp] Initial pose was not reached :(')
## Initialize action server
self._action_name = name
self._as = actionlib.SimpleActionServer( self._action_name, first_pr2_exp.msg.GoToPoseAction, execute_cb=self.execute_cb, auto_start = False )
self._as.start()
rospy.loginfo('[gtp] Action Server Initialized!')
# 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: Sachin Chitta, Jon Binney
import roslib;
roslib.load_manifest('pr2_plan_utils')
import rospy
from geometry_msgs.msg import PoseStamped
from pr2_plan_utils.arm_mover import ArmMover
# initialize
rospy.init_node('is_in_collision', anonymous=True)
right_arm = ArmMover('right_arm')
if right_arm.is_current_state_in_collision():
print 'Right arm in collision'
else:
print 'Right arm not in collision'
js = right_arm.get_joint_state()
js.position[0] = 1.0 #set shoulder pan (first joint) to swing into left arm
if right_arm.is_in_collision(js):
print 'Right arm in collision'
else:
print 'Right arm not in collision'
class BeAPoserAction(object):
#result msg
_result = first_pr2_exp.msg.GoToPoseResult()
def __init__(self, name):
self._arm_mover = ArmMover()
#self._tasks = Tasks()
self._arm_tasks = ArmTasks()
self._base = base.Base()
self._head = head.Head()
self._torso = torso.Torso()
self._tf = tf.TransformListener( True, rospy.Duration(100) )
self._tfBr = tf.TransformBroadcaster()
#################### MOVE TORSO TO MAX HEIGHT ####################
self._torso.up()
rospy.loginfo('[gtp] Torso lifted!')
#################### TUCK LEFT ARM #######################
#self._tasks.move_arms_to_side()
self._arm_tasks.move_arm_to_side('left_arm')
rospy.loginfo('[gtp] Left arm moved to the side!')
#action_name = 'tuck_arms'
#tuck_arms_action_server = tuck_arms_main.TuckArmsActionServer(action_name)
#tuck_arm_client = actionlib.SimpleActionClient(action_name, TuckArmsAction)
#rospy.logdebug('Waiting for tuckarm action server to start')
#tuck_arm_client.wait_for_server(rospy.Duration(10.0))
#rospy.logdebug('Tucking left arm...')
#goal = TuckArmsGoal()
#goal.tuck_left = True
#goal.tuck_right = False
#tuck_arm_client.send_goal_and_wait(goal, rospy.Duration(30.0), rospy.Duration(5.0))
#################### MOVE RIGHT ARM TO INITAL POSE #######################
#self._arm_tasks.move_arm_to_side('right_arm')
#rospy.loginfo('[gtp] Right arm moved to the side!')
self._init_arm_pose = PoseStamped()
self._init_arm_pose.pose.position.x = 0.282
self._init_arm_pose.pose.position.y = -0.485
self._init_arm_pose.pose.position.z = 0.303
self._init_arm_pose.pose.orientation.x = 0.030
self._init_arm_pose.pose.orientation.y = 0.342
self._init_arm_pose.pose.orientation.z = -0.057
self._init_arm_pose.pose.orientation.w = 0.937
self._init_arm_pose.header.frame_id = 'torso_lift_link'
for t in range(0,3):
handle = self._arm_mover.move_to_goal( 'right_arm', self._init_arm_pose,
collision_aware_goal=True, planner_timeout=10.,
bounds=(0.01, 0.01, 0.01, 0.1, 0.1, 0.1),
try_hard=False )
if handle.reached_goal():
break
if handle.reached_goal():
rospy.loginfo('[gtp] Right arm init pose reached!')
else:
rospy.logerr('[gtp] Moving right arm to init pose failed!')
rospy.logerr( handle.get_errors() )
#################### STRAIGHTEN HEAD #################
self._head.look_at_relative_point(0.8, 0.0, 0.5)
rospy.loginfo('[gtp] Head straightened!')
rospy.loginfo('[gtp] Attempting to reach initial pose...')
init_goal = self._get_init_goal()
success = self._go_to_pose( init_goal )
if success:
rospy.loginfo('[gtp] Initial pose successfully reached!')
else:
rospy.logwarn('[gtp] Initial pose was not reached :(')
## Initialize action server
self._action_name = name
self._as = actionlib.SimpleActionServer( self._action_name, first_pr2_exp.msg.GoToPoseAction, execute_cb=self.execute_cb, auto_start = False )
self._as.start()
rospy.loginfo('[gtp] Action Server Initialized!')
#
# _get_init_goal
#
def _get_init_goal(self):
#tabOrigin = numpy.array([0.98, 0.0, 0.55])
tabOrigin = numpy.array([1.475, 0.175, 0.85])
sensorStart = tabOrigin + numpy.array([-1.0, -0.0, 0.5])
offsets = numpy.array([0.0, -0.1, 0.1, -0.2, 0.2, -0.3, 0.3, -0.4, 0.4, -0.5, 0.5])
dirVec = tabOrigin - sensorStart
dirVec = dirVec / numpy.linalg.norm(dirVec)
yaw_pitch_roll = numpy.array([numpy.arctan2(dirVec[1], dirVec[0]),
-numpy.arctan2(dirVec[2], numpy.hypot(dirVec[0], dirVec[1])), 0.0 ])
# angle2quat
cyaw = numpy.cos(yaw_pitch_roll[0]/2)
cpitch = numpy.cos(yaw_pitch_roll[1]/2)
croll = numpy.cos(yaw_pitch_roll[2]/2)
syaw = numpy.sin(yaw_pitch_roll[0]/2)
spitch = numpy.sin(yaw_pitch_roll[1]/2)
sroll = numpy.sin(yaw_pitch_roll[2]/2)
sensorOrient = numpy.array([ cyaw*cpitch*sroll - syaw*spitch*croll,
cyaw*spitch*croll + syaw*cpitch*sroll,
syaw*cpitch*croll - cyaw*spitch*sroll,
cyaw*cpitch*croll + syaw*spitch*sroll ])
# end angle2quat
init_goal = first_pr2_exp.msg.GoToPoseGoal()
for i in range(len(offsets)):
init_goal.goal_pose_arr.append(PoseStamped())
init_goal.goal_pose_arr[i].pose.position.x = sensorStart[0] + offsets[i]*dirVec[0]
init_goal.goal_pose_arr[i].pose.position.y = sensorStart[1] + offsets[i]*dirVec[1]
init_goal.goal_pose_arr[i].pose.position.z = sensorStart[2] + offsets[i]*dirVec[2]
init_goal.goal_pose_arr[i].pose.orientation.x = sensorOrient[0]
init_goal.goal_pose_arr[i].pose.orientation.y = sensorOrient[1]
init_goal.goal_pose_arr[i].pose.orientation.z = sensorOrient[2]
init_goal.goal_pose_arr[i].pose.orientation.w = sensorOrient[3]
init_goal.goal_pose_arr[i].header.frame_id = "map"
init_goal.goal_pose_arr[i].header.stamp = rospy.Time.now()
return init_goal
#
# execute_cb
#
def execute_cb(self, goal):
rospy.loginfo('[gtp] Starting action execution!')
success = self._go_to_pose(goal)
if success:
rospy.loginfo('%s: Succeeded!' % (self._action_name) )
else:
rospy.loginfo('%s: Failed!' % (self._action_name) )
self._result.success = success
self._as.set_succeeded(self._result)
#
# _go_to_pose
#
def _go_to_pose(self, goal):
success = False
bnds = (0.02, 0.02, 0.02, 0.1, 0.1, 0.1)
num_arm_try = 3
# Compute the camera to wrist transform
while True:
try:
(o_t_w, o_q_w) = self._tf.lookupTransform('/arm_kinect_depth_optical_frame', '/r_wrist_roll_link', rospy.Time(0))
k_t_w = (o_t_w[2], -o_t_w[0], -o_t_w[1])
k_q_w = self._quatmult([-0.5, 0.5, -0.5, 0.5],o_q_w)
break
except:
rospy.logerr('[gtp] CANNOT LOOKUP camera to wrist transform')
#################################################################
########## Compute goal poses in /r_wrist_roll_link #############
wrist_pose_arr = []
for i in range(len(goal.goal_pose_arr)):
map_t_kinect = numpy.array([goal.goal_pose_arr[i].pose.position.x,
goal.goal_pose_arr[i].pose.position.y,
goal.goal_pose_arr[i].pose.position.z ])
map_R_kinect = self._quat2rot( goal.goal_pose_arr[i].pose.orientation.x,
goal.goal_pose_arr[i].pose.orientation.y,
goal.goal_pose_arr[i].pose.orientation.z,
goal.goal_pose_arr[i].pose.orientation.w )
map_t_wrist = numpy.dot(map_R_kinect, k_t_w) + map_t_kinect
map_q_wrist = self._quatmult([goal.goal_pose_arr[i].pose.orientation.x,
goal.goal_pose_arr[i].pose.orientation.y,
goal.goal_pose_arr[i].pose.orientation.z,
goal.goal_pose_arr[i].pose.orientation.w]
, k_q_w)
r_wrist_pose = goal.goal_pose_arr[i]
r_wrist_pose.pose.position.x = map_t_wrist[0]
r_wrist_pose.pose.position.y = map_t_wrist[1]
r_wrist_pose.pose.position.z = map_t_wrist[2]
r_wrist_pose.pose.orientation.x = map_q_wrist[0]
r_wrist_pose.pose.orientation.y = map_q_wrist[1]
r_wrist_pose.pose.orientation.z = map_q_wrist[2]
r_wrist_pose.pose.orientation.w = map_q_wrist[3]
r_wrist_pose.header.frame_id = 'map'
r_wrist_pose.header.stamp = rospy.Time.now()
wrist_pose_arr.append(r_wrist_pose)
rospy.loginfo('[gtp] Done determining the wrist poses')
#################################################################
## Try each of the poses with the arm first
needToMoveBase = True
for i in range(len(wrist_pose_arr)):
rospy.loginfo('[gtp] Trying pose %d of %d\n' % (i+1, len(wrist_pose_arr)) )
# Broadcast for visualization
self._broadcast_gpose( goal.goal_pose_arr[i] )
rospy.loginfo('[gtp] Move end-effector to map: (%f, %f, %f | %f, %f, %f, %f)' %
(wrist_pose_arr[i].pose.position.x,
wrist_pose_arr[i].pose.position.y,
wrist_pose_arr[i].pose.position.z,
wrist_pose_arr[i].pose.orientation.x,
wrist_pose_arr[i].pose.orientation.y,
wrist_pose_arr[i].pose.orientation.z,
wrist_pose_arr[i].pose.orientation.w) )
try:
for t in range(0,num_arm_try):
handle = self._arm_mover.move_to_goal( 'right_arm', wrist_pose_arr[i],
collision_aware_goal=True, planner_timeout=10.,
bounds=bnds, try_hard=False )
if handle.reached_goal():
break
if handle.reached_goal():
needToMoveBase = False
success = True
break
except:
rospy.loginfo('Arm mover exception for goal %d of %d\n' %
(i+1, len(wrist_pose_arr)) )
#################################################################
## If the goal was not reached, we need to move the base
if needToMoveBase:
rospy.loginfo('[gtp] Moving arm from current position failed, need to move base.')
# Raise arm first not to hit anything
for t in range(0,num_arm_try):
handle = self._arm_mover.move_to_goal('right_arm', self._init_arm_pose,
collision_aware_goal=True, planner_timeout=10.,
bounds=bnds, try_hard=False )
if handle.reached_goal():
break
for i in range(len(wrist_pose_arr)):
# Broadcast for visualization
self._broadcast_gpose( goal.goal_pose_arr[i] )
try:
self._base.move_manipulable_pose( wrist_pose_arr[i].pose.position.x,
wrist_pose_arr[i].pose.position.y,
wrist_pose_arr[i].pose.position.z,
'torso', False,
wrist_pose_arr[i].pose.orientation.x,
wrist_pose_arr[i].pose.orientation.y,
wrist_pose_arr[i].pose.orientation.z,
wrist_pose_arr[i].pose.orientation.w )
except:
rospy.logerr('EXCEPTION DUDE: Move base failed!!!!\n')
continue
# Base pose was reached try to move the arm now
rospy.loginfo('LALALBUBUBUB: DESIRED BASE POSE REACHED!');
try:
for t in range(0,num_arm_try):
handle = self._arm_mover.move_to_goal( 'right_arm', wrist_pose_arr[i],
collision_aware_goal=True, planner_timeout=10.,
bounds=bnds, try_hard=False )
if handle.reached_goal():
break
if handle.reached_goal():
success = True
break
except:
rospy.loginfo('Arm mover exception for goal %d of %d\n' %
(i+1, len(wrist_pose_arr)) )
return success
#
# Broadcasts a PoseStamped
#
def _broadcast_gpose(self, gpose):
self._tfBr.sendTransform( (gpose.pose.position.x,
gpose.pose.position.y,
gpose.pose.position.z),
(gpose.pose.orientation.x, gpose.pose.orientation.y,
gpose.pose.orientation.z, gpose.pose.orientation.w),
rospy.Time.now(),
"kinect_goal_pose", "/map" )
def _quat2rot(self, q0, q1, q2, q3):
R = numpy.array([[1.0-2.0*(q1*q1+q2*q2),
2.0*q0*q1-2.0*q3*q2,
2.0*q3*q1+2.0*q0*q2],
[2.0*q0*q1+2.0*q3*q2,
1.0-2.0*(q0*q0+q2*q2),
2.0*q1*q2-2.0*q3*q0],
[2.0*q0*q2-2.0*q3*q1,
2.0*q1*q2+2.0*q3*q0,
1.0-2.0*(q0*q0+q1*q1)]])
return R
def _quatmult(self, q1, q2):
x1, y1, z1, w1 = q1
x2, y2, z2, w2 = q2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
return x, y, z, w
# 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: Jon Binney
import roslib;
roslib.load_manifest('pr2_plan_utils')
import rospy
from geometry_msgs.msg import PoseStamped
from pr2_plan_utils.arm_mover import ArmMover
rospy.init_node('test_arm_control', anonymous=True)
arm_mover = ArmMover()
# Tell the left arm to move to a cartesian goal
goal_pose = PoseStamped()
goal_pose.pose.position.x = 0.65
goal_pose.pose.position.y = 0.2
goal_pose.pose.orientation.w = 1.0
goal_pose.header.frame_id = '/torso_lift_link'
handle1 = arm_mover.move_to_goal('left_arm', goal_pose, blocking=False)
# Tell the right arm to move to a joint space goal
joint_position = [-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338]
js = arm_mover.get_joint_state('right_arm')
js.position = joint_position
handle2 = arm_mover.move_to_goal('right_arm', js, blocking=False)
import roslib; roslib.load_manifest('pr2_plan_utils')
import rospy
from pr2_plan_utils.arm_mover import ArmMover
rospy.init_node('test_move_out_of_collision', anonymous=True)
arm_mover = ArmMover('right_arm')
arm_mover.move_out_of_collision(0.3)
#
# Author: Jon Binney
import roslib;
roslib.load_manifest('pr2_plan_utils')
import rospy
from geometry_msgs.msg import PoseStamped
from pr2_plan_utils.arm_mover import ArmMover
arm_name = 'right_arm'
# initialize
rospy.init_node('test_arm_control', anonymous=True)
arm_mover = ArmMover()
# generate desired arm pose
goal_pose = PoseStamped()
goal_pose.pose.position.x = 0.65
goal_pose.pose.orientation.w = 1.0
goal_pose.header.frame_id = '/torso_lift_link'
goal_pose.header.stamp = rospy.Time(0)
handle = arm_mover.move_to_goal_using_cartesian_control(arm_name, goal_pose, timeout=5.0, blocking=True)
if handle.reached_goal():
print 'Reached the goal!'
else:
print handle.get_errors()
# 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: Jon Binney
import roslib
roslib.load_manifest('pr2_plan_utils')
import rospy
from pr2_plan_utils.arm_mover import ArmMover
# initialize
rospy.init_node('test_arm_control', anonymous=True)
arm_mover = ArmMover()
# parameters
arm_name = 'right_arm'
joint_position = [-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338]
# get a joint state message already configured for this arm
js = arm_mover.get_joint_state(arm_name)
rospy.sleep(1.0)
# set desired joint positions
js.position = joint_position
print 'Moving to %s' % (str(joint_position))
# send out the command