def make_joint_traj(xyzs, quats, manip, ref_frame, targ_frame, filter_options = 0):
"do ik and then fill in the points where ik failed"
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
joint_inds = manip.GetArmIndices()
robot.SetActiveDOFs(joint_inds)
orig_joint = robot.GetActiveDOFValues()
joints = []
inds = []
for i in xrange(0,n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame, filter_options)
if joint is not None:
joints.append(joint)
inds.append(i)
robot.SetActiveDOFValues(joint)
robot.SetActiveDOFValues(orig_joint)
rospy.loginfo("found ik soln for %i of %i points",len(inds), n)
if len(inds) > 2:
joints2 = mu.interp2d(np.arange(n), inds, joints)
return joints2, inds
else:
return np.zeros((n, len(joints))), []
def make_joint_traj(xyzs, quats, joint_seeds, manip, ref_frame, targ_frame,
filter_options):
"""
do ik to make a joint trajectory
joint_seeds are the joint angles that this function will try to be close to
I put that in so you could try to stay near the demonstration joint angles
in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations
(TODO)
"""
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
robot.SetActiveManipulator(manip.GetName())
joint_inds = manip.GetArmIndices()
orig_joint = robot.GetDOFValues(joint_inds)
joints = []
inds = []
for i in xrange(0, n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
robot.SetDOFValues(joint_seeds[i], joint_inds)
joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame,
filter_options)
if joint is not None:
joints.append(joint)
inds.append(i)
robot.SetDOFValues(joint, joint_inds)
robot.SetDOFValues(orig_joint, joint_inds)
rospy.loginfo("found ik soln for %i of %i points", len(inds), n)
if len(inds) > 2:
joints2 = mu.interp2d(np.arange(n), inds, joints)
return joints2, inds
else:
return np.zeros((n, len(joints))), []
def make_joint_traj(xyzs, quats, joint_seeds,manip, ref_frame, targ_frame,filter_options):
"""
do ik to make a joint trajectory
joint_seeds are the joint angles that this function will try to be close to
I put that in so you could try to stay near the demonstration joint angles
in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations
(TODO)
"""
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
robot.SetActiveManipulator(manip.GetName())
joint_inds = manip.GetArmIndices()
orig_joint = robot.GetDOFValues(joint_inds)
joints = []
inds = []
for i in xrange(0,n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
robot.SetDOFValues(joint_seeds[i], joint_inds)
joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame, filter_options)
if joint is not None:
joints.append(joint)
inds.append(i)
robot.SetDOFValues(joint, joint_inds)
robot.SetDOFValues(orig_joint, joint_inds)
rospy.loginfo("found ik soln for %i of %i points",len(inds), n)
if len(inds) > 2:
joints2 = mu.interp2d(np.arange(n), inds, joints)
return joints2, inds
else:
return np.zeros((n, len(joints))), []
def make_joint_traj(xyzs,
quats,
manip,
ref_frame,
targ_frame,
filter_options=0):
"do ik and then fill in the points where ik failed"
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
joint_inds = manip.GetArmIndices()
robot.SetActiveDOFs(joint_inds)
orig_joint = robot.GetActiveDOFValues()
joints = []
inds = []
for i in xrange(0, n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame,
filter_options)
if joint is not None:
joints.append(joint)
inds.append(i)
robot.SetActiveDOFValues(joint)
robot.SetActiveDOFValues(orig_joint)
rospy.loginfo("found ik soln for %i of %i points", len(inds), n)
if len(inds) > 2:
joints2 = mu.interp2d(np.arange(n), inds, joints)
return joints2, inds
else:
return np.zeros((n, len(joints))), []
pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
pc_tf = ros_utils.transformPointCloud2(pc, pr2.tf_listener, "base_footprint", pc.header.frame_id)
n_sel = F[verb]["nargs"].value
arg_clouds = []
for i_sel in xrange(n_sel):
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc_tf)).cloud_out
xyz, rgb = ros_utils.pc2xyzrgb(pc_sel)
arg_clouds.append(xyz.reshape(-1,3))
make_and_execute_verb_traj(verb, arg_clouds)
raw_input("press enter to continue")
elif verb in ["swap-tool"]:
l_switch_posture = np.array([ 0.773, 0.595, 1.563, -1.433, 0.478, -0.862, .864])
lr = select_from_list(["left", "right"])
if lr == 'l':
arm, grip, jpos = pr2.larm, pr2.lgrip, l_switch_posture
else:
arm, grip, jpos = pr2.rarm, pr2.rgrip, PR2.mirror_arm_joints(l_switch_posture)
arm.goto_joint_positions(jpos)
grip.open()
raw_input("press enter when ready")
grip.close()
pr2.join_all()
pc.header.frame_id)
n_sel = F[verb]["nargs"].value
arg_clouds = []
for i_sel in xrange(n_sel):
pc_sel = seg_svc.call(
ProcessCloudRequest(cloud_in=pc_tf)).cloud_out
xyz, rgb = ros_utils.pc2xyzrgb(pc_sel)
arg_clouds.append(xyz.reshape(-1, 3))
make_and_execute_verb_traj(verb, arg_clouds)
raw_input("press enter to continue")
elif verb in ["swap-tool"]:
l_switch_posture = np.array(
[0.773, 0.595, 1.563, -1.433, 0.478, -0.862, .864])
lr = select_from_list(["left", "right"])
if lr == 'l':
arm, grip, jpos = pr2.larm, pr2.lgrip, l_switch_posture
else:
arm, grip, jpos = pr2.rarm, pr2.rgrip, PR2.mirror_arm_joints(
l_switch_posture)
arm.goto_joint_positions(jpos)
grip.open()
raw_input("press enter when ready")
grip.close()
pr2.join_all()
import utils.conversions as conv
from brett2.PR2 import *
#from brett2.PR2 import mirror_arm_joints
from brett2.ghost_PR2 import GhostPR2
from math import pi
import tf
import numpy
action_client = []
if __name__=='__main__':
rospy.init_node('ankush')
bot = PR2()
rospy.loginfo(bot.base.get_pose())
#bot.base.goto_pose(10,5.0,pi/2, '/map')
#bot.torso.go_up()
p = numpy.array([1,0,0])
bot.head.look_at(p, 'base_link', 'narrow_stereo_link')
fold_reset = [pi/32, pi/12, 0, -pi/2, pi , -pi/6, -pi/4]
bot.larm.goto_joint_positions(fold_reset)
bot.rarm.goto_joint_positions(mirror_arm_joints(fold_reset))
rospy.sleep(4)
bot.lgrip.open()
parser.add_argument("arms_used")
parser.add_argument("--dry_run", action="store_true")
args = parser.parse_args()
return args
if __name__ == "__main__":
if rospy.get_name() == "/unnamed":
rospy.init_node("multi_item_teach_verb", disable_signals=True)
args = get_args()
# arguments need to be: <verb> <items separated by ','> <arms_used separated by ',' and corresponding to items>
verb = args.verb
items_str = args.items
items = items_str.split(",")
arms_used = args.arms_used.split(",")
data_dir = osp.join(osp.dirname(lfd.__file__), "data")
pr2 = PR2.PR2()
move_pr2_to_start_pos(pr2)
demo_name = get_new_demo_name(verb, items)
stage_names_for_demo, special_pts = do_teach_verb(demo_name, items, arms_used, data_dir)
new_entry_text = get_new_demo_entry_text(demo_name, stage_names_for_demo, items, arms_used, special_pts)
yn = yes_or_no("save demonstration?")
# add the entry to the verbs yaml file
if yn:
add_new_entry_to_yaml_file(data_dir, new_entry_text)
else:
print "exiting without saving"
