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) # wait for moves to complete handle1.wait() handle2.wait() # check whether movements were successful if handle1.reached_goal(): print 'Left arm reached the goal' else: print handle1.get_errors() if handle2.reached_goal(): print 'Right arm reached the goal'
# 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'
# # 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 handle = arm_mover.move_to_goal(arm_name, js) if not handle.reached_goal(): print handle.get_errors()