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()
