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()
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
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,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()))
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)
#!/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()
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)