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 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 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')
def setup(): Globals.pr2 = PR2.create() Globals.rviz = RvizWrapper.create() rospy.sleep(1)
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"]))
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)
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)