Exemplo n.º 1
0
 def __init__ (self, init_pos=None, rave_items=False):
     PR2.__init__(self)
     self.rarm = PlannerArm(self,'r')
     self.larm = PlannerArm(self,'l')
     
     if init_pos is not None:
         try:
             self.gotoArmPosture(init_pos)
         except:
             rospy.logwarn ("Cannot go to pose " + str(init_pos))
             
     if rave_items:
         self.initRave()
Exemplo n.º 2
0
def callback(msg):
    s = msg.data
    print "message:", colorize(s, "blue", bold=True)
    action_arg = s.split()

    assert len(action_arg) == 2

    action, arg = action_arg

    if action == "relax":
        assert action_arg[0] == "relax"
        pr2 = PR2.create()
        pr2.rgrip.open()
        pr2.lgrip.open()
        pr2.rarm.goto_posture('side')
        pr2.larm.goto_posture('side')
    elif action == "tie":
        assert arg == "figure8_knot" or arg == "overhand_knot"
        call_and_print("execute_task.py --task=%s --count_steps" % arg)
    elif action == "teach":
        print colorize("which arms will be used?", "red", bold=True)
        arms_used = raw_input()
        call_and_print("teach_verb.py %s %s %s" % (arg, "object", arms_used))
    elif action == "execute":
        call_and_print("exec_one_demo_traj.py --verb=%s" % arg)
 def __init__(self):        
     self.brett = PR2()        
     self.sub = rospy.Subscriber("base_traj_controller/command", tm.JointTrajectory, self.callback)
     self.q = Queue()
     self.F = None
     self.stop_requested = False
     self.ctrl_loop_running = False
Exemplo n.º 4
0
def callback(msg):
    s = msg.data
    print "message:",colorize(s, "blue", bold=True)
    action_arg = s.split()
    
    
    assert len(action_arg) == 2
    
    action, arg = action_arg

    if action == "relax":
        assert action_arg[0] == "relax"
        pr2 = PR2.create()
        pr2.rgrip.open()
        pr2.lgrip.open()
        pr2.rarm.goto_posture('side')
        pr2.larm.goto_posture('side')
    elif action == "tie":
        assert arg == "figure8_knot" or arg == "overhand_knot"
        call_and_print("execute_task.py --task=%s --count_steps"%arg)
    elif action == "teach":
        print colorize("which arms will be used?", "red", bold=True)
        arms_used = raw_input()
        call_and_print("teach_verb.py %s %s %s"%(arg, "object", arms_used))
    elif action == "execute":
        call_and_print("exec_one_demo_traj.py --verb=%s"%arg)
Exemplo n.º 5
0
 def __init__ (self,initPos=None):
     PR2.__init__(self)
     self.rarm = PlannerArm(self,'r')
     self.larm = PlannerArm(self,'l')
     
     # In case needle is not added
     self.sneedle = None
     self.sneedle_radius = 0.112
     self.sneedle_pose = 0
     self.grabbingArm = None
     
     # In case sponge is not added (supporting sponge)
     self.sponge = None
     self.sponge_enabled = False
     
     if initPos is not None:
         try:
             self.gotoArmPosture(initPos)
         except:
             rospy.logwarn ("Cannot go to pose " + str(initPos))
             
     self.addTableToRave()
     self.addSpongeToRave()
     self.addNeedleToRave()
import sensor_msgs.msg as sm
import rospy
import brett2
from brett2.PR2 import PR2
import os.path as osp
import numpy as np

rospy.init_node("calibrate_gripper_closed")


pr2 = PR2()
pr2.rgrip.set_angle(-1)
pr2.lgrip.set_angle(-1)
pr2.join_all()
rospy.sleep(1)
data_dir = osp.join(osp.dirname(brett2.__file__),"data")
with open(osp.join(data_dir,"l_closed_val"),"w") as fh:
    fh.write(str(pr2.lgrip.get_angle()))
with open(osp.join(data_dir,"r_closed_val"),"w") as fh:
    fh.write(str(pr2.rgrip.get_angle()))
Exemplo n.º 7
0
def test_torso():
    brett.torso.go_down()
    brett.torso.go_up()
@testme
def test_grippers():
    brett.lgrip.close()
    brett.lgrip.open()
@testme
def test_base():
    brett.base.set_twist(0,1,.1)


if __name__ == "__main__":
    if rospy.get_name() == "/unnamed": # for ipython convenience
        rospy.init_node("test_pr2",disable_signals=True)
    
    brett = PR2.create()
    rospy.sleep(.5)
    
    #test_all()
    test_arm_ik()

#




#brett.larm.get_pose_matrix(
#posemat = brett.larm.get_pose_matrix('torso_lift_link','l_gripper_palm_link')
#brett.larm.goto_pose_matrix(posemat, 'torso_lift_link', 'l_gripper_palm_link')
Exemplo n.º 8
0
 def setup():
     Globals.pr2 = PR2.create()
     Globals.rviz = RvizWrapper.create()
     rospy.sleep(1)
Exemplo n.º 9
0
import h5py
from brett2.PR2 import PR2,IKFail
from brett2 import trajectories
from kinematics import kinematics_utils
import rospy
from brett2.ros_utils import RvizWrapper,Marker
from numpy import asarray
import numpy as np
import geometry_msgs.msg as gm
from utils import conversions

if rospy.get_name()=="/unnamed":
    rospy.init_node("playback_demo")

rviz = RvizWrapper.create()
pr2 = PR2.create()
rospy.sleep(1)


traj = h5py.File(args.file, 'r')[args.group]

ps = gm.PoseStamped(pose = gm.Pose(
    position = gm.Point(*traj["object_pose"]),
    orientation = gm.Quaternion(*traj["object_orientation"])))
ps.header.frame_id = 'base_link'
rviz.draw_marker(
    ps,
    id=1,
    type=Marker.CUBE,
    rgba = (0,1,0,1),
    scale = asarray(traj["object_dimension"]))
Exemplo n.º 10
0
import numpy as np
import sensor_msgs.msg as sm
import roslib
roslib.load_manifest("pr2_controllers_msgs")
import pr2_controllers_msgs.msg as pcm
import matplotlib.pyplot as plt
from brett2 import trajectories
import jds_utils.math_utils as mu
from brett2.PR2 import PR2, mirror_arm_joints

JOINT_NAME = "l_gripper_joint"
CMD_TOPIC = "/l_gripper_controller/command"
if rospy.get_name() == "/unnamed":
    rospy.init_node("check_command_vs_actual", disable_signals=True)

pr2 = PR2.create()
pr2.update_rave()
n_steps = 30
cmd_times = np.linspace(0, 3, n_steps)
cmd_states = np.zeros(n_steps, dtype=trajectories.BodyState)

r_arm_cur = pr2.rarm.get_joint_positions()
cmd_states["r_arm"] = mu.linspace2d(
    r_arm_cur,
    mirror_arm_joints(pr2.rarm.L_POSTURES["untucked"]) - r_arm_cur, n_steps)
l_arm_cur = pr2.larm.get_joint_positions()
cmd_states["l_arm"] = mu.linspace2d(l_arm_cur,
                                    pr2.rarm.L_POSTURES["tucked"] - l_arm_cur,
                                    n_steps)
r_grip_cur = pr2.rgrip.get_angle()
cmd_states["r_gripper"] = np.linspace(r_grip_cur, .08 - r_grip_cur, n_steps)
Exemplo n.º 11
0
#!/usr/bin/env python

import roslib

roslib.load_manifest('rospy')
import rospy

import utils.conversions as conv
from brett2.PR2 import PR2
from brett2.PR2 import mirror_arm_joints
import math
import numpy

if __name__ == '__main__':
    rospy.init_node('ankush')
    bot = PR2()

    bot.lgrip.open()
    bot.rgrip.open()
    fold_reset = [
        math.pi / 32, math.pi / 12, 0, -math.pi / 2, math.pi, -math.pi / 6,
        -math.pi / 4
    ]
    bot.larm.goto_joint_positions(fold_reset)
    bot.rarm.goto_joint_positions(mirror_arm_joints(fold_reset))

    rospy.spin()
Exemplo n.º 12
0
from brett2.PR2 import PR2
import jds_utils.conversions as conv
import numpy as np
import geometry_msgs.msg as gm
import trajectory_msgs.msg as tm
import scipy.interpolate as si
import rospy
from time import time, sleep
import roslib
from jds_utils.math_utils import linspace2d

if rospy.get_name() == "/unnamed":
    rospy.init_node("test_base_traj", anonymous=True, disable_signals=True)

brett = PR2.create()

n_steps = 10
ts = np.linspace(0, 1, n_steps)

xya_cur = np.r_[0, 0, 0]
xya_targ = np.r_[1, 0, 0]
xyas = linspace2d(xya_cur, xya_targ, n_steps)

pub = rospy.Publisher("base_traj_controller/command", tm.JointTrajectory)

jt = tm.JointTrajectory()
for i in xrange(10):

    jtp = tm.JointTrajectoryPoint()
    jtp.time_from_start = rospy.Duration(ts[i])
    jtp.positions = xyas[i]
	def __init__(self,
			table_height=None,
			grasp_pose_topic=None,
			grasp_pose_array_topic=None,
			table_height_topic=None,
			object_mesh_topic=None,
			object_point_cloud_topic=None,
			object_pose_topic=None,
			always_publish_pose=False,
			select_grasp=None,
			select_grasps_after=None,
			shuffle_poses=True):
		self.pr2 = PR2.create()
		
		rospy.loginfo("reseting")
		self.pr2.rarm.goto_posture('side')
		self.pr2.rgrip.open()
		
		
		self.base_frame = 'base_footprint'
		#self.tool_frame = 'r_gripper_tool_frame'
		self.tool_frame = 'r_wrist_roll_link'
		
		#self.move_arm = SimpleActionClient('move_right_arm', MoveArmAction)
		
		self.pregrasp_pose_pub = rospy.Publisher('pregrasp_pose',PoseStamped,latch=True);
		self.grasp_pose_pub = rospy.Publisher('grasp_pose',PoseStamped,latch=False);
		self.lift_pose_pub = rospy.Publisher('lift_pose',PoseStamped,latch=True);
		self.lift_rot_pose_pub = rospy.Publisher('lift_pose_rot',PoseStamped,latch=True);
		self.grasp_pose_array_pub = rospy.Publisher('grasp_pose_array',PoseArray,latch=True);
		
		self.grasp_num_pub = rospy.Publisher('grasp_num',Int32,latch=False)
		self.grasp_quality_pub = rospy.Publisher('grasp_quality',Float64,latch=False)
		self.success_pub = rospy.Publisher('success',Bool,latch=False)
		
		self.tracker_pause_pub = rospy.Publisher('tracker_pause',Bool);
		
		self.always_publish_pose = always_publish_pose
		self.select_grasp = select_grasp
		self.select_grasps_after = select_grasps_after
		self.shuffle_poses = shuffle_poses
		
		self.table_height = table_height
		self.table_height_topic = table_height_topic
		if self.table_height_topic:
			self.table_height_sub = rospy.Subscriber(self.table_height_topic, Float32, self.table_height_callback)
		
		self.use_object_mesh = False
		self.object_mesh = None
		
		self.use_object_pose = False
		self.object_pose = None
		self.object_mesh_topic = object_mesh_topic
		if self.object_mesh_topic:
			self.use_object_mesh = True
			self.object_mesh_sub = rospy.Subscriber(object_mesh_topic,PolygonMesh,self.object_mesh_callback)
		
		self.object_point_cloud_topic = object_point_cloud_topic
		if self.object_point_cloud_topic:
			#print 'subscribing to ', object_point_cloud_topic
			self.use_object_mesh = True
			self.cloud_to_mesh_srv_name = 'create_mesh'
			self.cloud_to_mesh_srv = None
			self.object_point_cloud_sub = rospy.Subscriber(object_point_cloud_topic,PointCloud2,self.object_point_cloud_callback)
		
		self.object_pose_topic = object_pose_topic
		if self.object_pose_topic:
			self.use_object_pose = True
			self.object_pose_sub = rospy.Subscriber(object_pose_topic,self.object_pose_callback)
		
		if grasp_pose_topic:
			self.grasp_pose_sub = rospy.Subscriber(grasp_pose_topic,PoseStamped,self.grasp_pose)
		
		if grasp_pose_array_topic:
			self.grasp_pose_array_sub = rospy.Subscriber(grasp_pose_array_topic,PoseArray,self.grasp_pose_array)