def main():
rospy.init_node("arm_cart_control_backend")
ctrl_switcher = ControllerSwitcher()
if False:
ctrl_switcher.carefree_switch(
'r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch(
'r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r',
PR2ArmJointTrajectory,
controller_name='%s_joint_controller_low')
q = [
-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314,
-1.82146607, -1.85187734
]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch(
'r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r',
PR2ArmCartesianPostureBase,
controller_name='%s_cart_low_rfh')
r_arm.set_posture()
rospy.sleep(0.2)
l_arm = None
#l_arm = create_pr2_arm('l', PR2ArmCartesianPostureBase)
#l_arm.set_posture()
rospy.sleep(0.2)
cart_ctrl = ArmCartCtrlBackend(r_arm, l_arm, MONITOR_RATE)
cart_ctrl.backend_loop()
def __init__(self):
rospy.init_node("ar_manipulation")
# rospy.Subscriber("/ar_pose_markers", ARMarkers, self.read_markers_cb)
# rospy.Subscriber("/ar_object_name", String, self.marker_lookup_cb)
rospy.Subscriber("/ar_object_name", StringArray, self.marker_lookup_cb)
rospy.Subscriber("/put_back_tool", String, self.put_back_tool_cb)
self.pub_rate = rospy.Rate(10)
self.torso = torso()
self.head = head()
self.gripper = gripper()
self.tf_listener = tf.TransformListener()
self.cs = pr2cs.ControllerSwitcher()
self.pr2_init = False
self.search_tag = False
self.found_tag = False
self.grasp_object = False
# Load JTcontroller
self.r_arm_cart = pr2arm.create_pr2_arm('r', pr2arm.PR2ArmJTranspose,
controller_name="%s_cart", timeout=0)
self.l_arm_cart = pr2arm.create_pr2_arm('l', pr2arm.PR2ArmJTranspose,
controller_name="%s_cart", timeout=0)
# Load Joint space controller
self.r_arm = pr2arm.create_pr2_arm('r', pr2arm.PR2ArmJointTrajectory,
controller_name="%s_arm_controller",
timeout=0)
self.l_arm = pr2arm.create_pr2_arm('l', pr2arm.PR2ArmJointTrajectory,
controller_name="%s_arm_controller",
timeout=0)
self.epc = eptc.EPC('linear_move')
self.time_step = 1/20.
def main():
rospy.init_node("arm_cart_vel_control")
if True:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch(
'r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch(
'r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r',
PR2ArmJointTrajectory,
controller_name='%s_joint_controller_low')
q = [
-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314,
-1.82146607, -1.85187734
]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch(
'r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase)
r_arm.set_posture()
rospy.sleep(0.2)
vel_ctrl = PR2ArmCartVelocityController(r_arm)
#vel_frame = tf_trans.euler_matrix(0, -np.pi/2, 0)[:3,:3]
#vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.005, is_translation=False)
vel_frame = tf_trans.euler_matrix(0, 0, np.pi / 2)[:3, :3]
vel_ctrl.move_velocity(velocity_rot=vel_frame,
velocity=0.10,
is_translation=False)
def __init__(self):
rospy.init_node("ar_manipulation")
# rospy.Subscriber("/ar_pose_markers", ARMarkers, self.read_markers_cb)
# rospy.Subscriber("/ar_object_name", String, self.marker_lookup_cb)
rospy.Subscriber("/ar_object_name", StringArray, self.marker_lookup_cb)
rospy.Subscriber("/put_back_tool", String, self.put_back_tool_cb)
self.pub_rate = rospy.Rate(10)
self.torso = torso()
self.head = head()
self.gripper = gripper()
self.tf_listener = tf.TransformListener()
self.cs = pr2cs.ControllerSwitcher()
self.pr2_init = False
self.search_tag = False
self.found_tag = False
self.grasp_object = False
# Load JTcontroller
self.r_arm_cart = pr2arm.create_pr2_arm('r', pr2arm.PR2ArmJTranspose)
self.l_arm_cart = pr2arm.create_pr2_arm('l', pr2arm.PR2ArmJTranspose)
# Load Joint space controller
self.r_arm = pr2arm.create_pr2_arm('r', pr2arm.PR2ArmJointTrajectory)
self.l_arm = pr2arm.create_pr2_arm('l', pr2arm.PR2ArmJointTrajectory)
self.epc = eptc.EPC('linear_move')
self.time_step = 1/20.
def main():
rospy.init_node("arm_cart_control_backend")
ctrl_switcher = ControllerSwitcher()
if False:
ctrl_switcher.carefree_switch('r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch('r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name='%s_joint_controller_low')
q = [-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314, -1.82146607, -1.85187734]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch('r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase, controller_name='%s_cart_low_rfh')
r_arm.set_posture()
rospy.sleep(0.2)
l_arm = None
#l_arm = create_pr2_arm('l', PR2ArmCartesianPostureBase)
#l_arm.set_posture()
rospy.sleep(0.2)
cart_ctrl = ArmCartCtrlBackend(r_arm, l_arm, MONITOR_RATE)
cart_ctrl.backend_loop()
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
rospy.sleep(1)
joint_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
setup_angles = [0, 0, np.pi/2, -np.pi/2, -np.pi, -np.pi/2, -np.pi/2]
#joint_controller = EPArmController(joint_arm, 0.1, "joint_ep_controller")
#joint_controller.execute_interpolated_ep(setup_angles, 10)
ctrl_switcher.carefree_switch('l', '%s_cart', None)
rospy.sleep(1)
cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask, end_link="%s_gripper_shaver45_frame")
cart_arm.set_posture(setup_angles)
#cart_arm.set_posture(cart_arm.get_joint_angles(wrapped=False))
cart_arm.set_gains([110, 600, 600, 40, 40, 40], [15, 15, 15, 1.2, 1.2, 1.2])
ell_controller = EllipsoidController(cart_arm)
ell_controller.reset_arm_orientation(8)
pg.init()
screen = pg.display.set_mode((300,300))
while not rospy.is_shutdown():
pg.event.get()
keys = pg.key.get_pressed()
dur = 5
if True:
r = np.random.randint(6)
if r == 0:
ell_controller.command_move([ 1, 0, 0], dur)
elif r == 1:
ell_controller.command_move([-1, 0, 0], dur)
elif r == 2:
ell_controller.command_move([ 0, 1, 0], dur)
elif r == 3:
ell_controller.command_move([ 0, -1, 0], dur)
elif r == 4:
ell_controller.command_move([ 0, 0, 1], dur)
elif r == 5:
ell_controller.command_move([ 0, 0, -1], dur)
rospy.sleep(1)
else:
if keys[pg.K_w]:
ell_controller.command_move([-1, 0, 0], dur)
if keys[pg.K_s]:
ell_controller.command_move([ 1, 0, 0], dur)
if keys[pg.K_a]:
ell_controller.command_move([ 0, 1, 0], dur)
if keys[pg.K_d]:
ell_controller.command_move([ 0, -1, 0], dur)
if keys[pg.K_q]:
ell_controller.command_move([ 0, 0, 1], dur)
if keys[pg.K_e]:
ell_controller.command_move([ 0, 0, -1], dur)
rospy.sleep(0.05)
def __init__(self, min_l_torques=[-5.]*7, min_r_torques=[-5.]*7,
max_l_torques=[5.]*7, max_r_torques=[5.]*7):
smach.State.__init__(self, outcomes=['collision', 'preempted', 'aborted'])
self.min_l_tor = np.array(min_l_torques)
self.min_r_tor = np.array(min_r_torques)
self.max_l_tor = np.array(max_l_torques)
self.max_r_tor = np.array(max_r_torques)
self.l_arm = create_pr2_arm('l', PR2ArmJointTrajectory, timeout=0)
self.r_arm = create_pr2_arm('r', PR2ArmJointTrajectory, timeout=0)
def __init__(self,
min_l_torques=[-5.] * 7,
min_r_torques=[-5.] * 7,
max_l_torques=[5.] * 7,
max_r_torques=[5.] * 7):
smach.State.__init__(self,
outcomes=['collision', 'preempted', 'aborted'])
self.min_l_tor = np.array(min_l_torques)
self.min_r_tor = np.array(min_r_torques)
self.max_l_tor = np.array(max_l_torques)
self.max_r_tor = np.array(max_r_torques)
self.l_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
self.r_arm = create_pr2_arm('r', PR2ArmJointTrajectory)
def setup_servoing_arms(msg):
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('r', joint_ctrl, reset=False)
ctrl_switcher.carefree_switch('l', joint_ctrl, reset=False)
r_arm = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name=joint_ctrl)
l_arm = create_pr2_arm('l', PR2ArmJointTrajectory, controller_name=joint_ctrl)
r_ep_arm_ctrl = EPArmController(r_arm)
l_ep_arm_ctrl = EPArmController(l_arm)
r_ep_arm_ctrl.execute_interpolated_ep([-1.91, 1.25, -1.93, -1.53, 0.33, -0.03, 0.0],
15, blocking=False)
l_ep_arm_ctrl.execute_interpolated_ep([1.91, 1.25, 1.93, -1.53, -0.33, -0.03, -3.09],
15, blocking=True)
return EmptyResponse()
def __init__(self, interface_backend):
self.backend = interface_backend()
self.cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask,
controller_name='%s_cart_jt_task',
end_link="%s_gripper_shaver45_frame")
self.backend.set_arm(self.cart_arm)
rospy.loginfo("[ell_control_experiment] EllipsoidControlExperiment ready.")
def pose_saver(tf_frame, bag_file, pickle_file):
arm = create_pr2_arm('l', PR2ArmJTransposeTask)
tf_list = tf.TransformListener()
named_poses = NamedPoses()
named_poses.pose_arr.header.frame_id = tf_frame
pose_dict = {}
while True:
if rospy.is_shutdown():
return
pose_name = raw_input(
"Type the name of the pose and hit enter to save it (Type 'q' to quit) "
)
if pose_name == 'q':
break
arm_pose = PoseConverter.to_pose_stamped_msg(
"/torso_lift_link", arm.get_end_effector_pose())
tf_pose = tf_list.transformPose(tf_frame, arm_pose)
named_poses.pose_arr.poses.append(tf_pose)
named_poses.names.append(pose_name)
pose_dict[pose_name] = tf_pose
if bag_file != "":
bag = rosbag.Bag(bag_file, 'w')
bag.write("/named_poses", named_poses)
bag.close()
if pickle_file != "":
f = file(pickle_file, 'w')
pickle.dump(pose_dict, f)
f.close()
def pose_saver(tf_frame, bag_file, pickle_file):
arm = create_pr2_arm('l', PR2ArmJTransposeTask)
tf_list = tf.TransformListener()
named_poses = NamedPoses()
named_poses.pose_arr.header.frame_id = tf_frame
pose_dict = {}
while True:
if rospy.is_shutdown():
return
pose_name = raw_input("Type the name of the pose and hit enter to save it (Type 'q' to quit) ")
if pose_name == 'q':
break
arm_pose = PoseConverter.to_pose_stamped_msg("/torso_lift_link", arm.get_end_effector_pose())
tf_pose = tf_list.transformPose(tf_frame, arm_pose)
named_poses.pose_arr.poses.append(tf_pose)
named_poses.names.append(pose_name)
pose_dict[pose_name] = tf_pose
if bag_file != "":
bag = rosbag.Bag(bag_file, 'w')
bag.write("/named_poses", named_poses)
bag.close()
if pickle_file != "":
f = file(pickle_file, 'w')
pickle.dump(pose_dict, f)
f.close()
def tool_arm_setup(self):
self.ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
rospy.sleep(1)
joint_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
setup_angles = SETUP_ANGLES
joint_arm.set_ep(setup_angles, 5)
rospy.sleep(6)
def __init__(self):
self.tf_listener = tf.TransformListener()
self.start_frame_pub = rospy.Publisher("~start_frame", PoseStamped)
self.end_frame_pub = rospy.Publisher("~end_frame", PoseStamped)
self.arm = create_pr2_arm('l', PR2ArmHybridForce)
self.tf_pub = TFPubLoop("/torso_lift_link", "/contact_control_frame")
def __init__(self):
self.tf_listener = tf.TransformListener()
self.start_frame_pub = rospy.Publisher("~start_frame", PoseStamped)
self.end_frame_pub = rospy.Publisher("~end_frame", PoseStamped)
self.arm = create_pr2_arm('l', PR2ArmHybridForce)
self.tf_pub = TFPubLoop("/torso_lift_link", "/contact_control_frame")
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
ctrl_switcher.carefree_switch('r', '%s_arm_controller', None)
rospy.sleep(1)
l_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
r_arm = create_pr2_arm('r', PR2ArmJointTrajectory)
l_tuck_angles = [ 0.14228106, 1.29643293, 1.78480255, -1.56470338, 1.16505304,
-0.09788312, 0.23542476]
r_tuck_angles = [ 0.01289596, 1.02437885, -1.34551339, -1.78272859, 0.38331793,
-1.28334274, 0.02605728]
l_joint_controller = EPArmController(l_arm, 0.1, "l_joint_ep_controller")
r_joint_controller = EPArmController(r_arm, 0.1, "r_joint_ep_controller")
l_joint_controller.execute_interpolated_ep(l_tuck_angles, 10)
r_joint_controller.execute_interpolated_ep(r_tuck_angles, 10)
def __init__(self, interface_backend):
self.backend = interface_backend()
self.cart_arm = create_pr2_arm('l',
PR2ArmJTransposeTask,
controller_name='%s_cart_jt_task',
end_link="%s_gripper_shaver45_frame")
self.backend.set_arm(self.cart_arm)
rospy.loginfo(
"[ell_control_experiment] EllipsoidControlExperiment ready.")
def main():
rospy.init_node("teleop_positioner")
from optparse import OptionParser
p = OptionParser()
p.add_option('-r', '--rate', dest="rate", default=10, help="Set rate.")
(opts, args) = p.parse_args()
arm = create_pr2_arm('l', PR2ArmHybridForce)
rospy.sleep(0.1)
arm.zero_sensor()
cur_pose = arm.get_end_effector_pose()
arm.set_ep(cur_pose, 1)
arm.set_force_directions([])
arm.set_force_gains(p_trans=[3, 1, 1],
p_rot=0.5,
i_trans=[0.002, 0.001, 0.001],
i_max_trans=[10, 5, 5],
i_rot=0,
i_max_rot=0)
arm.set_motion_gains(p_trans=400,
d_trans=[16, 10, 10],
p_rot=[10, 10, 10],
d_rot=0)
arm.set_tip_frame("/l_gripper_tool_frame")
arm.update_gains()
arm.set_force(6 * [0])
r = rospy.Rate(float(opts.rate))
while not rospy.is_shutdown():
ep_pose = arm.get_ep()
cur_pose = arm.get_end_effector_pose()
err_ep = arm.ep_error(cur_pose, ep_pose)
if np.linalg.norm(err_ep[0:3]) > 0.012 or np.linalg.norm(
err_ep[3:]) > np.pi / 8.:
arm.set_ep(cur_pose, 1)
r.sleep()
cur_pose = arm.get_end_effector_pose()
arm.set_ep(cur_pose, 1)
q = arm.get_joint_angles()
q_posture = q.tolist()[0:3] + 4 * [9999]
arm.set_posture(q_posture)
arm.set_motion_gains(p_trans=400,
d_trans=[16, 10, 10],
p_rot=[20, 50, 50],
d_rot=0)
arm.update_gains()
print PoseConverter.to_pos_quat(cur_pose)
pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"])
bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'w')
bag.write("/teleop_pose", PoseConverter.to_pose_msg(cur_pose))
q_posture_msg = Float64MultiArray()
q_posture_msg.data = q_posture
bag.write("/teleop_posture", q_posture_msg)
bag.close()
def cart_controller_setup(self):
self.ctrl_switcher.carefree_switch('l', '%s_cart_jt_task',
"$(find hrl_rfh_fall_2011)/params/l_jt_task_shaver45.yaml")
self.cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask,
controller_name='%s_cart_jt_task',
end_link="%s_gripper_shaver45_frame")
rospy.sleep(2)
setup_angles = SETUP_ANGLES
self.cart_arm.set_posture(setup_angles)
self.cart_arm.set_gains([200, 800, 800, 80, 80, 80], [15, 15, 15, 1.2, 1.2, 1.2])
def mirror_arm_setup(self):
self.ctrl_switcher.carefree_switch('r', 'r_joint_controller_mirror',
"$(find hrl_ellipsoidal_control)/params/mirror_params.yaml")
rospy.sleep(1)
arm = create_pr2_arm('r', PR2ArmJointTrajectory,
controller_name="r_joint_controller_mirror")
arm.set_ep([-0.26880036055585677, 0.71881299774143248,
-0.010187938126536471, -1.43747589322259,
-12.531293698878677, -0.92339874393497123,
3.3566322715405432], 5)
rospy.sleep(6)
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
ctrl_switcher.carefree_switch('r', '%s_arm_controller', None)
rospy.sleep(1)
l_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
r_arm = create_pr2_arm('r', PR2ArmJointTrajectory)
l_tuck_angles = [
0.14228106, 1.29643293, 1.78480255, -1.56470338, 1.16505304,
-0.09788312, 0.23542476
]
r_tuck_angles = [
0.01289596, 1.02437885, -1.34551339, -1.78272859, 0.38331793,
-1.28334274, 0.02605728
]
l_joint_controller = EPArmController(l_arm, 0.1, "l_joint_ep_controller")
r_joint_controller = EPArmController(r_arm, 0.1, "r_joint_ep_controller")
l_joint_controller.execute_interpolated_ep(l_tuck_angles, 10)
r_joint_controller.execute_interpolated_ep(r_tuck_angles, 10)
def main():
rospy.init_node("ellipsoid_controller", sys.argv)
if sys.argv[1] == '-s':
setup_task_controller = SetupTaskController()
setup_task_controller.execute(None)
cart_arm = setup_task_controller.arm
else:
cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask, end_link="%s_gripper_shaver45_frame")
rospy.sleep(1)
ell_controller = EllipsoidController(cart_arm)
rospy.spin()
def mirror_mannequin(self):
arm = create_pr2_arm('r', PR2ArmJointTrajectory,
controller_name="r_joint_controller_mirror")
r = rospy.Rate(10)
q_act_last = arm.get_joint_angles()
while not rospy.is_shutdown():
q_act = arm.get_joint_angles()
q_ep = arm.get_ep()
new_ep = q_ep.copy()
for i in range(7):
if np.fabs(q_act[i] - q_act_last[i]) > joint_deltas[i]:
new_ep[i] = q_act[i]
arm.set_ep(new_ep, 0.1)
q_act_last = q_act
r.sleep()
def main():
rospy.init_node("ellipsoid_controller", sys.argv)
if sys.argv[1] == '-s':
setup_task_controller = SetupTaskController()
setup_task_controller.execute(None)
cart_arm = setup_task_controller.arm
else:
cart_arm = create_pr2_arm('l',
PR2ArmJTransposeTask,
end_link="%s_gripper_shaver45_frame")
rospy.sleep(1)
ell_controller = EllipsoidController(cart_arm)
rospy.spin()
def main():
rospy.init_node("arm_cart_vel_control")
if True:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch('r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name='%s_joint_controller_low')
q = [-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314, -1.82146607, -1.85187734]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch('r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase)
r_arm.set_posture()
rospy.sleep(0.2)
vel_ctrl = PR2ArmCartVelocityController(r_arm)
#vel_frame = tf_trans.euler_matrix(0, -np.pi/2, 0)[:3,:3]
#vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.005, is_translation=False)
vel_frame = tf_trans.euler_matrix(0, 0, np.pi/2)[:3,:3]
vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.10, is_translation=False)
def main():
rospy.init_node("teleop_positioner")
from optparse import OptionParser
p = OptionParser()
p.add_option('-r', '--rate', dest="rate", default=10,
help="Set rate.")
(opts, args) = p.parse_args()
arm = create_pr2_arm('l', PR2ArmHybridForce)
rospy.sleep(0.1)
arm.zero_sensor()
cur_pose = arm.get_end_effector_pose()
arm.set_ep(cur_pose, 1)
arm.set_force_directions([])
arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.5, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0)
arm.set_motion_gains(p_trans=400, d_trans=[16, 10, 10], p_rot=[10, 10, 10], d_rot=0)
arm.set_tip_frame("/l_gripper_tool_frame")
arm.update_gains()
arm.set_force(6 * [0])
r = rospy.Rate(float(opts.rate))
while not rospy.is_shutdown():
ep_pose = arm.get_ep()
cur_pose = arm.get_end_effector_pose()
err_ep = arm.ep_error(cur_pose, ep_pose)
if np.linalg.norm(err_ep[0:3]) > 0.012 or np.linalg.norm(err_ep[3:]) > np.pi / 8.:
arm.set_ep(cur_pose, 1)
r.sleep()
cur_pose = arm.get_end_effector_pose()
arm.set_ep(cur_pose, 1)
q = arm.get_joint_angles()
q_posture = q.tolist()[0:3] + 4 * [9999]
arm.set_posture(q_posture)
arm.set_motion_gains(p_trans=400, d_trans=[16, 10, 10], p_rot=[20, 50, 50], d_rot=0)
arm.update_gains()
print PoseConverter.to_pos_quat(cur_pose)
pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"])
bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'w')
bag.write("/teleop_pose", PoseConverter.to_pose_msg(cur_pose))
q_posture_msg = Float64MultiArray()
q_posture_msg.data = q_posture
bag.write("/teleop_posture", q_posture_msg)
bag.close()
def main():
rospy.init_node("pr2_arm_test")
arm = sys.argv[1]
jnt_arm = create_pr2_arm(arm, arm_type=PR2ArmJointTrajectory)
kin = jnt_arm.kinematics
ellipse_rot = np.mat([[-1., 0., 0.], [0., -1., 0.], [0., 0., 1.]])
sspace = SpheroidSpace(0.15, np.mat([0.78, -0.18, 0.1]).T, ellipse_rot)
uvp = np.array([1.0, np.pi/2, 0.0])
uvp_delta = np.array([0.0, 0.6, 0.6])
pos, rot = sspace.spheroidal_to_pose(uvp + uvp_delta)
print pos, rot
#q_ik = kin.IK_search(pos, rot)
q_ik = kin.IK(pos, rot, jnt_arm.get_joint_angles())
if q_ik is not None:
jnt_arm.set_ep(q_ik, 5)
else:
print "IK failure"
def collect_data_pr2(n_4, n_5, n_6):
roslib.load_manifest("hrl_pr2_arms")
from hrl_pr2_arms.pr2_arm import PR2ArmJointTrajectory, create_pr2_arm
netft_topic = rospy.get_param("~netft_topic")
gravity_frame = rospy.get_param("~gravity_frame")
netft_frame = rospy.get_param("~netft_frame")
jnt_arm = create_pr2_arm('l', arm_type=PR2ArmJointTrajectory)
kin = jnt_arm.kinematics
netft_list = NetFTListener(netft_topic)
tf_list = tf.TransformListener()
q_setup = [0.4, 0.0, 1.0, -1.2, 0, -0.1, -3.14]
q_4_vals = np.linspace(0, 4.7, n_4)
q_5_vals = np.linspace(-0.1, -1.8, n_5)
q_6_vals = np.linspace(-3.14, -0.2, n_6)
jnt_arm.set_ep(q_setup, 8)
rospy.sleep(8.)
data = []
q_cur = q_setup
for q_4 in q_4_vals:
for q_5 in q_5_vals:
for q_6 in q_6_vals:
q_cur[4] = q_4
q_cur[5] = q_5
q_cur[6] = q_6
jnt_arm.set_ep(q_cur, 2.)
rospy.sleep(4.)
(trans, rot) = tf_list.lookupTransform(gravity_frame,
netft_frame,
rospy.Time(0))
data.append((copy.copy(netft_list.cur_wrench), rot))
if rospy.is_shutdown():
return
print "Wrench, orientation data", data
return data
def collect_data_pr2(n_4, n_5, n_6):
roslib.load_manifest("hrl_pr2_arms")
from hrl_pr2_arms.pr2_arm import PR2ArmJointTrajectory, create_pr2_arm
netft_topic = rospy.get_param("~netft_topic")
gravity_frame = rospy.get_param("~gravity_frame")
netft_frame = rospy.get_param("~netft_frame")
jnt_arm = create_pr2_arm('l', arm_type=PR2ArmJointTrajectory)
kin = jnt_arm.kinematics
netft_list = NetFTListener(netft_topic)
tf_list = tf.TransformListener()
q_setup = [0.4, 0.0, 1.0, -1.2, 0, -0.1, -3.14]
q_4_vals = np.linspace(0, 4.7, n_4)
q_5_vals = np.linspace(-0.1, -1.8, n_5)
q_6_vals = np.linspace(-3.14, -0.2, n_6)
jnt_arm.set_ep(q_setup, 8)
rospy.sleep(8.)
data = []
q_cur = q_setup
for q_4 in q_4_vals:
for q_5 in q_5_vals:
for q_6 in q_6_vals:
q_cur[4] = q_4
q_cur[5] = q_5
q_cur[6] = q_6
jnt_arm.set_ep(q_cur, 2.)
rospy.sleep(4.)
(trans, rot) = tf_list.lookupTransform(gravity_frame, netft_frame, rospy.Time(0))
data.append((copy.copy(netft_list.cur_wrench), rot))
if rospy.is_shutdown():
return
print "Wrench, orientation data", data
return data
def main():
rospy.init_node("cool_force_demo")
from optparse import OptionParser
p = OptionParser()
p.add_option('-f', '--force_mag', dest="force_mag", default=2,
help="Specify force magnitude.")
p.add_option('-x', '--max_force', dest="max_force", default=-1,
help="Specify max force magnitude.")
p.add_option('-c', '--compliance', dest="compliance", default=-1,
help="Compliance to maintain.")
p.add_option('-t', '--tip_frame', dest="tip_frame", default="/l_gripper_tool_frame",
help="Set tip to this frame.")
p.add_option('-z', '--zero_sensor', dest="zero_sensor", default=False,
action="store_true", help="Just zero the sensor.")
p.add_option('-l', '--force_line', dest="force_line", default=False,
action="store_true", help="Move in a line with zero force.")
p.add_option('-p', '--force_plane', dest="force_plane", default=False,
action="store_true", help="Move in a plane with zero force.")
p.add_option('-o', '--force_point', dest="force_point", default=False,
action="store_true", help="Move about a point with zero torque.")
p.add_option('-r', '--force_roll', dest="force_roll", default=False,
action="store_true", help="Move the wrist with zero torque.")
p.add_option('-a', '--force_all', dest="force_all", default=False,
action="store_true", help="All DOF are trying to set zero force.")
p.add_option('-n', '--force_none', dest="force_none", default=False,
action="store_true", help="Return to position control.")
p.add_option('-k', '--kill_controller', dest="kill_controller", default=False,
action="store_true", help="Render controller dead.")
(opts, args) = p.parse_args()
arm = create_pr2_arm('l', PR2ArmHybridForce)
rospy.sleep(1)
if opts.zero_sensor:
arm.zero_sensor()
# reset arm
arm.set_ep(arm.get_end_effector_pose(), 0)
arm.set_force(6 * [0])
fp_trans_major = 3
fp_trans_minor = 1
fp_rot = 0.1
fi_trans_major = 0.002
fi_trans_minor = 0.001
fi_max_trans_major = 10
fi_max_trans_minor = 5
fi_rot = 0
fi_max_rot = 0
pp_trans = 1000
pd_trans_major = 16
pd_trans_minor = 4
pp_rot = 120
pd_rot = 0
if opts.force_line:
arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(p_trans=[fp_trans_major, fp_trans_minor, fp_trans_minor],
p_rot=fp_rot, i_trans=[fi_trans_major, fi_trans_minor, fi_trans_minor],
i_max_trans=[fi_max_trans_major, fi_max_trans_minor, fi_max_trans_minor],
i_rot=fi_rot, i_max_rot=fi_max_rot)
arm.set_motion_gains(p_trans=[float(opts.compliance), pp_trans, pp_trans],
d_trans=[pd_trans_major, pd_trans_minor, pd_trans_minor],
p_rot=pp_rot, d_rot=pd_rot)
arm.set_force_directions(['x'])
arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0])
arm.update_gains()
return
if opts.force_plane:
arm.set_force_max([-1, float(opts.max_force), float(opts.max_force), -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(p_trans=[fp_trans_minor, fp_trans_major, fp_trans_major],
p_rot=fp_rot, i_trans=[fi_trans_minor, fi_trans_major, fi_trans_major],
i_max_trans=[fi_max_trans_minor, fi_max_trans_major, fi_max_trans_major],
i_rot=fi_rot, i_max_rot=fi_max_rot)
arm.set_motion_gains(p_trans=[pp_trans, float(opts.compliance), float(opts.compliance)],
d_trans=[pd_trans_minor, pd_trans_major, pd_trans_major],
p_rot=pp_rot, d_rot=pd_rot)
arm.set_force_directions(['y', 'z'])
arm.set_force([0, float(opts.force_mag), float(opts.force_mag), 0, 0, 0])
arm.update_gains()
return
if opts.force_point:
arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(p_trans=[fp_trans_minor, fp_trans_minor, fp_trans_minor],
p_rot=0.8, i_trans=[fi_trans_minor, fi_trans_minor, fi_trans_minor],
i_max_trans=[fi_max_trans_minor, fi_max_trans_minor, fi_max_trans_minor],
i_rot=fi_rot, i_max_rot=fi_max_rot)
arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans],
d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor],
p_rot=pp_rot, d_rot=pd_rot)
arm.set_force_directions([0, 0, 0, 1, 1, 1])
arm.set_force([0, 0, 0, float(opts.force_mag), float(opts.force_mag), float(opts.force_mag)])
arm.update_gains()
return
if opts.force_roll:
arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(p_trans=[fp_trans_minor, fp_trans_minor, fp_trans_minor],
p_rot=1.8, i_trans=[fi_trans_minor, fi_trans_minor, fi_trans_minor],
i_max_trans=[fi_max_trans_minor, fi_max_trans_minor, fi_max_trans_minor],
i_rot=fi_rot, i_max_rot=fi_max_rot)
arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans],
d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor],
p_rot=pp_rot, d_rot=pd_rot)
arm.set_force_directions([0, 0, 0, 1, 0, 0])
arm.set_force([0, 0, 0, float(opts.force_mag), 0, 0])
arm.update_gains()
return
if opts.force_all:
arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(p_trans=6,
p_rot=1.8, i_trans=[fi_trans_major, fi_trans_major, fi_trans_major],
i_max_trans=[fi_max_trans_major, fi_max_trans_major, fi_max_trans_major],
i_rot=fi_rot, i_max_rot=fi_max_rot)
arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans],
d_trans=[pd_trans_major, pd_trans_major, pd_trans_major],
p_rot=pp_rot, d_rot=pd_rot)
arm.set_force_directions([1, 1, 1, 1, 1, 1])
arm.set_force([0, 0, 0, 0, 0, 0])
arm.update_gains()
return
if opts.force_none:
arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(p_trans=[0, 0, 0],
p_rot=0, i_trans=[0, 0, 0],
i_max_trans=[0, 0, 0],
i_rot=0, i_max_rot=0)
arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans],
d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor],
p_rot=pp_rot, d_rot=pd_rot)
arm.set_force_directions([0, 0, 0, 0, 0, 0])
arm.set_force([0, 0, 0, 0, 0, 0])
arm.update_gains()
return
if opts.kill_controller:
arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(p_trans=[0, 0, 0],
p_rot=0, i_trans=[0, 0, 0],
i_max_trans=[0, 0, 0],
i_rot=0, i_max_rot=0)
arm.set_motion_gains(p_trans=0,
d_trans=0,
p_rot=0, d_rot=0)
arm.set_force_directions([0, 0, 0, 0, 0, 0])
arm.set_force([0, 0, 0, 0, 0, 0])
arm.update_gains()
return
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded'])
self.ctrl_switcher = ControllerSwitcher()
self.arm = create_pr2_arm('l', PR2ArmJTransposeTask, end_link="%s_gripper_shaver45_frame")
def main():
rospy.init_node("switch_controller")
from optparse import OptionParser
p = OptionParser()
p.add_option('-s',
'--stiff',
dest="stiff",
default=False,
action="store_true",
help="Enable stiff controller.")
p.add_option('-f',
'--force',
dest="force",
default=False,
action="store_true",
help="Enable force controller.")
p.add_option('-m',
'--force_mag',
dest="force_mag",
default=2,
help="Specify force magnitude.")
p.add_option('-x',
'--max_force',
dest="max_force",
default=-1,
help="Specify max force magnitude.")
p.add_option('-i',
'--impedance',
dest="impedance",
default=False,
action="store_true",
help="Enable impedance controller.")
p.add_option('-c',
'--compliance',
dest="compliance",
default=-1,
help="Enable impedance controller.")
p.add_option('-t',
'--tip_frame',
dest="tip_frame",
default="/l_gripper_tool_frame",
help="Set tip to this frame.")
p.add_option('-z',
'--zero_sensor',
dest="zero_sensor",
default=False,
action="store_true",
help="Just zero the sensor.")
p.add_option('-r',
'--reset_pose',
dest="reset_pose",
default=False,
action="store_true",
help="Use the saved position in the data file.")
(opts, args) = p.parse_args()
arm = create_pr2_arm('l', PR2ArmHybridForce)
rospy.sleep(0.1)
# reset arm
arm.zero_sensor()
if opts.zero_sensor:
return
arm.set_force(6 * [0])
#
if opts.reset_pose:
pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"])
bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'r')
for topic, msg, stamp in bag.read_messages("/teleop_pose"):
pose = PoseConverter.to_pos_rot(
[msg.position.x, msg.position.y, msg.position.z], [
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
])
for topic, msg, stamp in bag.read_messages("/teleop_posture"):
posture = msg.data
bag.close()
arm.set_posture(posture)
i_poses = arm.interpolate_ep(arm.get_end_effector_pose(), pose, 100)
for cur_pose in i_poses:
arm.set_ep(cur_pose, 1)
rospy.sleep(0.1)
return
# set common parameters
arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
if opts.stiff:
compliance = float(opts.compliance)
if compliance < 0:
compliance = 1300
#arm.set_force_gains(p_trans=[1, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0)
arm.set_force_gains(p_trans=[1, 0, 0],
p_rot=0.1,
i_trans=[0.002, 0, 0],
i_max_trans=[10, 0, 0],
i_rot=0,
i_max_rot=0)
arm.set_motion_gains(p_trans=[compliance, 1300, 1300],
d_trans=[16, 10, 10],
p_rot=120,
d_rot=0)
arm.set_force_directions([])
arm.set_force(6 * [0])
elif opts.impedance:
compliance = float(opts.compliance)
if compliance < 0:
compliance = 80
arm.set_force_gains(p_trans=[3, 1, 1],
p_rot=0.1,
i_trans=[0.002, 0.001, 0.001],
i_max_trans=[10, 5, 5],
i_rot=0,
i_max_rot=0)
arm.set_motion_gains(p_trans=[compliance, 1300, 1300],
d_trans=[16, 10, 10],
p_rot=120,
d_rot=0)
arm.set_force_directions([])
arm.set_force(6 * [0])
elif opts.force:
arm.set_force_gains(p_trans=[3, 1, 1],
p_rot=0.1,
i_trans=[0.002, 0.001, 0.001],
i_max_trans=[10, 5, 5],
i_rot=0,
i_max_rot=0)
arm.set_motion_gains(p_trans=[float(opts.compliance), 1300, 1300],
d_trans=[16, 10, 10],
p_rot=120,
d_rot=0)
arm.set_force_directions(['x'])
arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0])
else:
p.print_help()
return
arm.update_gains()
def main():
rospy.init_node("cool_force_demo")
from optparse import OptionParser
p = OptionParser()
p.add_option('-f',
'--force_mag',
dest="force_mag",
default=2,
help="Specify force magnitude.")
p.add_option('-x',
'--max_force',
dest="max_force",
default=-1,
help="Specify max force magnitude.")
p.add_option('-c',
'--compliance',
dest="compliance",
default=-1,
help="Compliance to maintain.")
p.add_option('-t',
'--tip_frame',
dest="tip_frame",
default="/l_gripper_tool_frame",
help="Set tip to this frame.")
p.add_option('-z',
'--zero_sensor',
dest="zero_sensor",
default=False,
action="store_true",
help="Just zero the sensor.")
p.add_option('-l',
'--force_line',
dest="force_line",
default=False,
action="store_true",
help="Move in a line with zero force.")
p.add_option('-p',
'--force_plane',
dest="force_plane",
default=False,
action="store_true",
help="Move in a plane with zero force.")
p.add_option('-o',
'--force_point',
dest="force_point",
default=False,
action="store_true",
help="Move about a point with zero torque.")
p.add_option('-r',
'--force_roll',
dest="force_roll",
default=False,
action="store_true",
help="Move the wrist with zero torque.")
p.add_option('-a',
'--force_all',
dest="force_all",
default=False,
action="store_true",
help="All DOF are trying to set zero force.")
p.add_option('-n',
'--force_none',
dest="force_none",
default=False,
action="store_true",
help="Return to position control.")
p.add_option('-k',
'--kill_controller',
dest="kill_controller",
default=False,
action="store_true",
help="Render controller dead.")
(opts, args) = p.parse_args()
arm = create_pr2_arm('l', PR2ArmHybridForce)
rospy.sleep(1)
if opts.zero_sensor:
arm.zero_sensor()
# reset arm
arm.set_ep(arm.get_end_effector_pose(), 0)
arm.set_force(6 * [0])
fp_trans_major = 3
fp_trans_minor = 1
fp_rot = 0.1
fi_trans_major = 0.002
fi_trans_minor = 0.001
fi_max_trans_major = 10
fi_max_trans_minor = 5
fi_rot = 0
fi_max_rot = 0
pp_trans = 1000
pd_trans_major = 16
pd_trans_minor = 4
pp_rot = 120
pd_rot = 0
if opts.force_line:
arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(
p_trans=[fp_trans_major, fp_trans_minor, fp_trans_minor],
p_rot=fp_rot,
i_trans=[fi_trans_major, fi_trans_minor, fi_trans_minor],
i_max_trans=[
fi_max_trans_major, fi_max_trans_minor, fi_max_trans_minor
],
i_rot=fi_rot,
i_max_rot=fi_max_rot)
arm.set_motion_gains(
p_trans=[float(opts.compliance), pp_trans, pp_trans],
d_trans=[pd_trans_major, pd_trans_minor, pd_trans_minor],
p_rot=pp_rot,
d_rot=pd_rot)
arm.set_force_directions(['x'])
arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0])
arm.update_gains()
return
if opts.force_plane:
arm.set_force_max(
[-1,
float(opts.max_force),
float(opts.max_force), -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(
p_trans=[fp_trans_minor, fp_trans_major, fp_trans_major],
p_rot=fp_rot,
i_trans=[fi_trans_minor, fi_trans_major, fi_trans_major],
i_max_trans=[
fi_max_trans_minor, fi_max_trans_major, fi_max_trans_major
],
i_rot=fi_rot,
i_max_rot=fi_max_rot)
arm.set_motion_gains(
p_trans=[pp_trans,
float(opts.compliance),
float(opts.compliance)],
d_trans=[pd_trans_minor, pd_trans_major, pd_trans_major],
p_rot=pp_rot,
d_rot=pd_rot)
arm.set_force_directions(['y', 'z'])
arm.set_force(
[0, float(opts.force_mag),
float(opts.force_mag), 0, 0, 0])
arm.update_gains()
return
if opts.force_point:
arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(
p_trans=[fp_trans_minor, fp_trans_minor, fp_trans_minor],
p_rot=0.8,
i_trans=[fi_trans_minor, fi_trans_minor, fi_trans_minor],
i_max_trans=[
fi_max_trans_minor, fi_max_trans_minor, fi_max_trans_minor
],
i_rot=fi_rot,
i_max_rot=fi_max_rot)
arm.set_motion_gains(
p_trans=[pp_trans, pp_trans, pp_trans],
d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor],
p_rot=pp_rot,
d_rot=pd_rot)
arm.set_force_directions([0, 0, 0, 1, 1, 1])
arm.set_force([
0, 0, 0,
float(opts.force_mag),
float(opts.force_mag),
float(opts.force_mag)
])
arm.update_gains()
return
if opts.force_roll:
arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(
p_trans=[fp_trans_minor, fp_trans_minor, fp_trans_minor],
p_rot=1.8,
i_trans=[fi_trans_minor, fi_trans_minor, fi_trans_minor],
i_max_trans=[
fi_max_trans_minor, fi_max_trans_minor, fi_max_trans_minor
],
i_rot=fi_rot,
i_max_rot=fi_max_rot)
arm.set_motion_gains(
p_trans=[pp_trans, pp_trans, pp_trans],
d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor],
p_rot=pp_rot,
d_rot=pd_rot)
arm.set_force_directions([0, 0, 0, 1, 0, 0])
arm.set_force([0, 0, 0, float(opts.force_mag), 0, 0])
arm.update_gains()
return
if opts.force_all:
arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(
p_trans=6,
p_rot=1.8,
i_trans=[fi_trans_major, fi_trans_major, fi_trans_major],
i_max_trans=[
fi_max_trans_major, fi_max_trans_major, fi_max_trans_major
],
i_rot=fi_rot,
i_max_rot=fi_max_rot)
arm.set_motion_gains(
p_trans=[pp_trans, pp_trans, pp_trans],
d_trans=[pd_trans_major, pd_trans_major, pd_trans_major],
p_rot=pp_rot,
d_rot=pd_rot)
arm.set_force_directions([1, 1, 1, 1, 1, 1])
arm.set_force([0, 0, 0, 0, 0, 0])
arm.update_gains()
return
if opts.force_none:
arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(p_trans=[0, 0, 0],
p_rot=0,
i_trans=[0, 0, 0],
i_max_trans=[0, 0, 0],
i_rot=0,
i_max_rot=0)
arm.set_motion_gains(
p_trans=[pp_trans, pp_trans, pp_trans],
d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor],
p_rot=pp_rot,
d_rot=pd_rot)
arm.set_force_directions([0, 0, 0, 0, 0, 0])
arm.set_force([0, 0, 0, 0, 0, 0])
arm.update_gains()
return
if opts.kill_controller:
arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
arm.set_force_gains(p_trans=[0, 0, 0],
p_rot=0,
i_trans=[0, 0, 0],
i_max_trans=[0, 0, 0],
i_rot=0,
i_max_rot=0)
arm.set_motion_gains(p_trans=0, d_trans=0, p_rot=0, d_rot=0)
arm.set_force_directions([0, 0, 0, 0, 0, 0])
arm.set_force([0, 0, 0, 0, 0, 0])
arm.update_gains()
return
import numpy as np
import roslib
roslib.load_manifest("hrl_pr2_arms")
import rospy
import rosparam
import tf.transformations as tf_trans
from hrl_pr2_arms.pr2_arm import create_pr2_arm, PR2ArmJointTrajectory, PR2ArmJTransposeTask
from hrl_pr2_arms.pr2_arm_hybrid import PR2ArmHybridForce
rospy.init_node("ipython_node")
arm = create_pr2_arm('l', PR2ArmHybridForce)
# motion gains p - proportional, d - derivative
# t - translational, r - rotational
kpt, kpr, kdt, kdr = 100, 10, 10, 1
kfpt, kfpr = 1, 0.3 # force gains
force_selector = [1, 0, 0, 0, 0,
0] # control force in x direction, motion in others
arm.set_motion_gains(kpt, kpr, kdt, kdr)
arm.set_force_gains(kfpt, kfpr)
arm.update_gains()
# force desired is currently at 0
if False:
arm.set_force([1, 0, 0, 0, 0, 0])
# apply 1N in x direction (can only command values in x currently)
import numpy as np
import roslib
roslib.load_manifest("hrl_pr2_arms")
import rospy
import rosparam
import tf.transformations as tf_trans
from hrl_pr2_arms.pr2_arm import create_pr2_arm, PR2ArmJointTrajectory, PR2ArmJTransposeTask
from hrl_pr2_arms.pr2_arm_hybrid import PR2ArmHybridForce
rospy.init_node("ipython_node")
arm = create_pr2_arm('l', PR2ArmHybridForce)
# motion gains p - proportional, d - derivative
# t - translational, r - rotational
kpt, kpr, kdt, kdr = 100, 10, 10, 1
kfpt, kfpr = 1, 0.3 # force gains
force_selector = [1, 0, 0, 0, 0, 0] # control force in x direction, motion in others
arm.set_motion_gains(kpt, kpr, kdt, kdr)
arm.set_force_gains(kfpt, kfpr)
arm.update_gains()
# force desired is currently at 0
if False:
arm.set_force([1, 0, 0, 0, 0, 0])
# apply 1N in x direction (can only command values in x currently)
def main():
rospy.init_node("pr2_arm_test")
arm = sys.argv[1]
mode = sys.argv[2]
assert arm in ['r', 'l']
assert mode in ['joint1', 'joint2', 'cart1', 'cart2', 'cart3', 'cart4']
if mode == 'joint1':
pr2_joint_arm = create_pr2_arm(arm, arm_type=PR2ArmJointTrajectory)
pr2_joint_arm.set_ep([0.1] * 7, 15)
if mode == 'joint2':
jnt_arm = create_pr2_arm(arm, arm_type=PR2ArmJointTrajectory)
kin = jnt_arm.kinematics
pos = np.mat([[0.6, 0.0, 0.0]]).T
rot = np.mat(np.eye(3))
q_ik = kin.IK_search(pos, rot)
if q_ik is not None:
jnt_arm.set_ep(q_ik, 15)
if mode == 'cart1':
pr2_r_kinematics = PR2ArmKinematics(arm)
pr2_jtrans_arm = PR2ArmJTranspose(arm, pr2_r_kinematics)
pos = np.mat([[0.6, 0.0, 0.0]]).T
rot = np.mat(np.eye(3))
pr2_jtrans_arm.set_ep((pos, rot), 5.)
if mode == 'cart2':
pr2_r_kinematics = PR2ArmKinematics(arm)
pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics)
pr2_jttask_arm.set_gains([0., 60., 60., 50., 50., 50.],
[3., 0.1, 0.1, 3., 3., 3.],
'r_gripper_tool_frame')
if mode == 'cart3':
pr2_r_kinematics = PR2ArmKinematics(arm)
pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics)
pos_a = np.mat([[0.0, 0.1, 0.0]]).T
rot_a = np.mat(np.eye(3))
pos_b = np.mat([[0.6, 0.0, 0.0]]).T
rot_b = np.mat([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]])
print zip(
*pr2_jttask_arm.interpolate_ep((pos_a, rot_a), (pos_b, rot_b), 10))
if mode == 'cart4':
pr2_r_kinematics = PR2ArmKinematics(arm)
pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics)
pos_a = np.mat([[0.0, 0.1, 0.0]]).T
rot_a = np.mat(tf_trans.random_rotation_matrix())[:3, :3]
pos_b = np.mat([[0.6, 0.0, 0.0]]).T
rot_b = np.mat(tf_trans.euler_matrix(0.5, 0.8, 0.5))[:3, :3]
ep_err = pr2_jttask_arm.ep_error((pos_a, rot_a), (pos_b, rot_b))
err_mat = np.mat(
tf_trans.euler_matrix(ep_err[3, 0], ep_err[4, 0],
ep_err[5, 0]))[:3, :3]
print "err_mat", err_mat
diff_mat = rot_b.T * rot_a
print "diff_mat", diff_mat
rx, ry, rz = tf_trans.euler_from_matrix(diff_mat)
print ep_err[3:6, 0].T
print rx, ry, rz
print diff_mat - err_mat
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded'])
self.ctrl_switcher = ControllerSwitcher()
self.arm = create_pr2_arm('l',
PR2ArmJTransposeTask,
end_link="%s_gripper_shaver45_frame")
def __init__(self):
rospy.init_node('left_arm_actions')
self.tfl = TransformListener()
self.aul = l_arm_utils.ArmUtils(self.tfl)
#self.fth = ft_handler.FTHandler()
rospy.loginfo("Waiting for l_utility_frame_service")
try:
rospy.wait_for_service('/l_utility_frame_update', 7.0)
self.aul.update_frame = rospy.ServiceProxy('/l_utility_frame_update', FrameUpdate)
rospy.loginfo("Found l_utility_frame_service")
except:
rospy.logwarn("Left Utility Frame Service Not available")
rospy.loginfo('Waiting for Pixel_2_3d Service')
try:
rospy.wait_for_service('/pixel_2_3d', 7.0)
self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True)
rospy.loginfo("Found pixel_2_3d service")
except:
rospy.logwarn("Pixel_2_3d Service Not available")
#Low-level motion requests: pass directly to arm_utils
rospy.Subscriber('wt_left_arm_pose_commands', Point, self.torso_frame_move)
rospy.Subscriber('wt_left_arm_angle_commands', JointTrajectoryPoint, self.aul.send_traj_point)
#More complex motion scripts, defined here using arm_util functions
rospy.Subscriber('norm_approach_left', PoseStamped, self.safe_approach)
#rospy.Subscriber('norm_approach_left', PoseStamped, self.hfc_approach)
rospy.Subscriber('wt_grasp_left_goal', PoseStamped, self.grasp)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.wipe)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.force_wipe_agg)
rospy.Subscriber('wt_hfc_swipe_left_goals', PoseStamped, self.hfc_swipe)
rospy.Subscriber('wt_lin_move_left', Float32, self.hand_move)
rospy.Subscriber('wt_adjust_elbow_left', Float32, self.aul.adjust_elbow)
rospy.Subscriber('wt_surf_wipe_left_points', Point, self.force_surf_wipe)
rospy.Subscriber('wt_poke_left_point', PoseStamped, self.poke)
rospy.Subscriber('wt_contact_approach_left', PoseStamped, self.approach_to_contact)
rospy.Subscriber('wt_hfc_contact_approach_left', PoseStamped, self.hfc_approach_to_contact)
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_pose = rospy.Publisher('test_pose', PoseStamped, latch=True)
self.say = rospy.Publisher('text_to_say', String)
self.wipe_started = False
self.surf_wipe_started = False
self.wipe_ends = [PoseStamped(), PoseStamped()]
#FORCE_TORQUE ADDITIONS
#rospy.Subscriber('netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('l_cart/ft_wrench', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('wt_cont_switch', String, self.cont_switch)
#self.rezero_wrench = rospy.Publisher('netft_gravity_zeroing/rezero_wrench', Bool)
self.rezero_wrench = rospy.Publisher('l_cart/ft_zero', Bool)
#rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.ft_preprocess)
#rospy.Subscriber('wt_ft_goal', Float32, self.set_force_goal)
rospy.Subscriber('wt_ft_goal', Float32, self.hfc_set_force)
self.wt_force_out = rospy.Publisher('wt_force_out', Float32)
self.ft_rate_limit = rospy.Rate(30)
self.ft = WrenchStamped()
self.ft_mag = 0.
self.ft_mag_que = deque([0]*10,10)
self.ft_sm_mag = 0.
self.ft_case = None
self.force_goal_mean = 1.42
self.force_goal_std= 0.625
self.stop_maintain = False
self.force_wipe_started = False
self.force_wipe_start = PoseStamped()
self.force_wipe_appr = PoseStamped()
################### HFC ###################
rospy.Subscriber('wt_hfc_mannequin', Bool, self.hfc_mannequin)
self.active_cont = 'l_arm_controller'
self.cs = ControllerSwitcher()
self.arm = create_pr2_arm('l', PR2ArmHybridForce)
# motion gains p - proportional, d - derivative
# t - translational, r - rotational
#kpt, kpr, kdt, kdr = 300, 8, 15, 4.2
#kfpt, kfpr, kfdt, kfdr = 3.8, 0.3, 0.1, 0.1 # force gains (derivative damps velocity)
#force_selector = [1, 0, 0, 0, 0, 0] # control force in x direction, motion in others
#self.arm.set_gains([kpt, kpt, kpt, kpr, kpr, kpr], [kdt, kdt, kdt, kdr, kdr, kdr], [kfpt, kfpt, kfpt, kfpr, kfpr, kfpr], [kfdt, kfdt, kfdt, kfdr, kfdr, kfdr], force_selector)
# force desired is currently at 0
self.hfc_pose_out = rospy.Publisher('/l_cart/command_pose', PoseStamped)
rospy.Subscriber('l_cart/state/x', PoseStamped, self.hfc_cur_pose)
def main():
rospy.init_node("pr2_arm_test")
arm = sys.argv[1]
mode = sys.argv[2]
assert arm in ['r', 'l']
assert mode in ['joint1', 'joint2', 'cart1', 'cart2', 'cart3', 'cart4']
if mode == 'joint1':
pr2_joint_arm = create_pr2_arm(arm, arm_type=PR2ArmJointTrajectory)
pr2_joint_arm.set_ep([0.1]*7, 15)
if mode == 'joint2':
jnt_arm = create_pr2_arm(arm, arm_type=PR2ArmJointTrajectory)
kin = jnt_arm.kinematics
pos = np.mat([[0.6, 0.0, 0.0]]).T
rot = np.mat(np.eye(3))
q_ik = kin.IK_search(pos, rot)
if q_ik is not None:
jnt_arm.set_ep(q_ik, 15)
if mode == 'cart1':
pr2_r_kinematics = PR2ArmKinematics(arm)
pr2_jtrans_arm = PR2ArmJTranspose(arm, pr2_r_kinematics)
pos = np.mat([[0.6, 0.0, 0.0]]).T
rot = np.mat(np.eye(3))
pr2_jtrans_arm.set_ep((pos, rot), 5.)
if mode == 'cart2':
pr2_r_kinematics = PR2ArmKinematics(arm)
pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics)
pr2_jttask_arm.set_gains([0., 60., 60., 50., 50., 50.],
[3., 0.1, 0.1, 3., 3., 3.], 'r_gripper_tool_frame')
if mode == 'cart3':
pr2_r_kinematics = PR2ArmKinematics(arm)
pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics)
pos_a = np.mat([[0.0, 0.1, 0.0]]).T
rot_a = np.mat(np.eye(3))
pos_b = np.mat([[0.6, 0.0, 0.0]]).T
rot_b = np.mat([[1.0, 0.0, 0.0],
[0.0, 0.0, -1.0],
[0.0, 1.0, 0.0]])
print zip(*pr2_jttask_arm.interpolate_ep((pos_a, rot_a), (pos_b, rot_b), 10))
if mode == 'cart4':
pr2_r_kinematics = PR2ArmKinematics(arm)
pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics)
pos_a = np.mat([[0.0, 0.1, 0.0]]).T
rot_a = np.mat(tf_trans.random_rotation_matrix())[:3,:3]
pos_b = np.mat([[0.6, 0.0, 0.0]]).T
rot_b = np.mat(tf_trans.euler_matrix(0.5, 0.8, 0.5))[:3,:3]
ep_err = pr2_jttask_arm.ep_error((pos_a, rot_a), (pos_b, rot_b))
err_mat = np.mat(tf_trans.euler_matrix(ep_err[3,0], ep_err[4,0], ep_err[5,0]))[:3, :3]
print "err_mat", err_mat
diff_mat = rot_b.T * rot_a
print "diff_mat", diff_mat
rx, ry, rz = tf_trans.euler_from_matrix(diff_mat)
print ep_err[3:6,0].T
print rx, ry, rz
print diff_mat - err_mat
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
rospy.sleep(1)
joint_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
setup_angles = [
0, 0, np.pi / 2, -np.pi / 2, -np.pi, -np.pi / 2, -np.pi / 2
]
#joint_controller = EPArmController(joint_arm, 0.1, "joint_ep_controller")
#joint_controller.execute_interpolated_ep(setup_angles, 10)
ctrl_switcher.carefree_switch('l', '%s_cart', None)
rospy.sleep(1)
cart_arm = create_pr2_arm('l',
PR2ArmJTransposeTask,
end_link="%s_gripper_shaver45_frame")
cart_arm.set_posture(setup_angles)
#cart_arm.set_posture(cart_arm.get_joint_angles(wrapped=False))
cart_arm.set_gains([110, 600, 600, 40, 40, 40],
[15, 15, 15, 1.2, 1.2, 1.2])
ell_controller = EllipsoidController(cart_arm)
ell_controller.reset_arm_orientation(8)
pg.init()
screen = pg.display.set_mode((300, 300))
while not rospy.is_shutdown():
pg.event.get()
keys = pg.key.get_pressed()
dur = 5
if True:
r = np.random.randint(6)
if r == 0:
ell_controller.command_move([1, 0, 0], dur)
elif r == 1:
ell_controller.command_move([-1, 0, 0], dur)
elif r == 2:
ell_controller.command_move([0, 1, 0], dur)
elif r == 3:
ell_controller.command_move([0, -1, 0], dur)
elif r == 4:
ell_controller.command_move([0, 0, 1], dur)
elif r == 5:
ell_controller.command_move([0, 0, -1], dur)
rospy.sleep(1)
else:
if keys[pg.K_w]:
ell_controller.command_move([-1, 0, 0], dur)
if keys[pg.K_s]:
ell_controller.command_move([1, 0, 0], dur)
if keys[pg.K_a]:
ell_controller.command_move([0, 1, 0], dur)
if keys[pg.K_d]:
ell_controller.command_move([0, -1, 0], dur)
if keys[pg.K_q]:
ell_controller.command_move([0, 0, 1], dur)
if keys[pg.K_e]:
ell_controller.command_move([0, 0, -1], dur)
rospy.sleep(0.05)
def __init__(self):
rospy.init_node('left_arm_actions')
self.tfl = TransformListener()
self.aul = l_arm_utils.ArmUtils(self.tfl)
#self.fth = ft_handler.FTHandler()
rospy.loginfo("Waiting for l_utility_frame_service")
try:
rospy.wait_for_service('/l_utility_frame_update', 7.0)
self.aul.update_frame = rospy.ServiceProxy(
'/l_utility_frame_update', FrameUpdate)
rospy.loginfo("Found l_utility_frame_service")
except:
rospy.logwarn("Left Utility Frame Service Not available")
rospy.loginfo('Waiting for Pixel_2_3d Service')
try:
rospy.wait_for_service('/pixel_2_3d', 7.0)
self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True)
rospy.loginfo("Found pixel_2_3d service")
except:
rospy.logwarn("Pixel_2_3d Service Not available")
#Low-level motion requests: pass directly to arm_utils
rospy.Subscriber('wt_left_arm_pose_commands', Point,
self.torso_frame_move)
rospy.Subscriber('wt_left_arm_angle_commands', JointTrajectoryPoint,
self.aul.send_traj_point)
#More complex motion scripts, defined here using arm_util functions
rospy.Subscriber('norm_approach_left', PoseStamped, self.safe_approach)
#rospy.Subscriber('norm_approach_left', PoseStamped, self.hfc_approach)
rospy.Subscriber('wt_grasp_left_goal', PoseStamped, self.grasp)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.wipe)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped,
self.force_wipe_agg)
rospy.Subscriber('wt_hfc_swipe_left_goals', PoseStamped,
self.hfc_swipe)
rospy.Subscriber('wt_lin_move_left', Float32, self.hand_move)
rospy.Subscriber('wt_adjust_elbow_left', Float32,
self.aul.adjust_elbow)
rospy.Subscriber('wt_surf_wipe_left_points', Point,
self.force_surf_wipe)
rospy.Subscriber('wt_poke_left_point', PoseStamped, self.poke)
rospy.Subscriber('wt_contact_approach_left', PoseStamped,
self.approach_to_contact)
rospy.Subscriber('wt_hfc_contact_approach_left', PoseStamped,
self.hfc_approach_to_contact)
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_pose = rospy.Publisher('test_pose', PoseStamped, latch=True)
self.say = rospy.Publisher('text_to_say', String)
self.wipe_started = False
self.surf_wipe_started = False
self.wipe_ends = [PoseStamped(), PoseStamped()]
#FORCE_TORQUE ADDITIONS
#rospy.Subscriber('netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('l_cart/ft_wrench', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('wt_cont_switch', String, self.cont_switch)
#self.rezero_wrench = rospy.Publisher('netft_gravity_zeroing/rezero_wrench', Bool)
self.rezero_wrench = rospy.Publisher('l_cart/ft_zero', Bool)
#rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.ft_preprocess)
#rospy.Subscriber('wt_ft_goal', Float32, self.set_force_goal)
rospy.Subscriber('wt_ft_goal', Float32, self.hfc_set_force)
self.wt_force_out = rospy.Publisher('wt_force_out', Float32)
self.ft_rate_limit = rospy.Rate(30)
self.ft = WrenchStamped()
self.ft_mag = 0.
self.ft_mag_que = deque([0] * 10, 10)
self.ft_sm_mag = 0.
self.ft_case = None
self.force_goal_mean = 1.42
self.force_goal_std = 0.625
self.stop_maintain = False
self.force_wipe_started = False
self.force_wipe_start = PoseStamped()
self.force_wipe_appr = PoseStamped()
################### HFC ###################
rospy.Subscriber('wt_hfc_mannequin', Bool, self.hfc_mannequin)
self.active_cont = 'l_arm_controller'
self.cs = ControllerSwitcher()
self.arm = create_pr2_arm('l', PR2ArmHybridForce)
# motion gains p - proportional, d - derivative
# t - translational, r - rotational
#kpt, kpr, kdt, kdr = 300, 8, 15, 4.2
#kfpt, kfpr, kfdt, kfdr = 3.8, 0.3, 0.1, 0.1 # force gains (derivative damps velocity)
#force_selector = [1, 0, 0, 0, 0, 0] # control force in x direction, motion in others
#self.arm.set_gains([kpt, kpt, kpt, kpr, kpr, kpr], [kdt, kdt, kdt, kdr, kdr, kdr], [kfpt, kfpt, kfpt, kfpr, kfpr, kfpr], [kfdt, kfdt, kfdt, kfdr, kfdr, kfdr], force_selector)
# force desired is currently at 0
self.hfc_pose_out = rospy.Publisher('/l_cart/command_pose',
PoseStamped)
rospy.Subscriber('l_cart/state/x', PoseStamped, self.hfc_cur_pose)
def main():
rospy.init_node("switch_controller")
from optparse import OptionParser
p = OptionParser()
p.add_option('-s', '--stiff', dest="stiff", default=False,
action="store_true", help="Enable stiff controller.")
p.add_option('-f', '--force', dest="force", default=False,
action="store_true", help="Enable force controller.")
p.add_option('-m', '--force_mag', dest="force_mag", default=2,
help="Specify force magnitude.")
p.add_option('-x', '--max_force', dest="max_force", default=-1,
help="Specify max force magnitude.")
p.add_option('-i', '--impedance', dest="impedance", default=False,
action="store_true", help="Enable impedance controller.")
p.add_option('-c', '--compliance', dest="compliance", default=-1,
help="Enable impedance controller.")
p.add_option('-t', '--tip_frame', dest="tip_frame", default="/l_gripper_tool_frame",
help="Set tip to this frame.")
p.add_option('-z', '--zero_sensor', dest="zero_sensor", default=False,
action="store_true", help="Just zero the sensor.")
p.add_option('-r', '--reset_pose', dest="reset_pose", default=False,
action="store_true", help="Use the saved position in the data file.")
(opts, args) = p.parse_args()
arm = create_pr2_arm('l', PR2ArmHybridForce)
rospy.sleep(0.1)
# reset arm
arm.zero_sensor()
if opts.zero_sensor:
return
arm.set_force(6 * [0])
#
if opts.reset_pose:
pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"])
bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'r')
for topic, msg, stamp in bag.read_messages("/teleop_pose"):
pose = PoseConverter.to_pos_rot([msg.position.x, msg.position.y, msg.position.z],
[msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w])
for topic, msg, stamp in bag.read_messages("/teleop_posture"):
posture = msg.data
bag.close()
arm.set_posture(posture)
i_poses = arm.interpolate_ep(arm.get_end_effector_pose(), pose, 100)
for cur_pose in i_poses:
arm.set_ep(cur_pose, 1)
rospy.sleep(0.1)
return
# set common parameters
arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1])
arm.set_tip_frame(opts.tip_frame)
if opts.stiff:
compliance = float(opts.compliance)
if compliance < 0:
compliance = 1300
#arm.set_force_gains(p_trans=[1, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0)
arm.set_force_gains(p_trans=[1, 0, 0], p_rot=0.1, i_trans=[0.002, 0, 0], i_max_trans=[10, 0, 0], i_rot=0, i_max_rot=0)
arm.set_motion_gains(p_trans=[compliance, 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0)
arm.set_force_directions([])
arm.set_force(6 * [0])
elif opts.impedance:
compliance = float(opts.compliance)
if compliance < 0:
compliance = 80
arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0)
arm.set_motion_gains(p_trans=[compliance, 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0)
arm.set_force_directions([])
arm.set_force(6 * [0])
elif opts.force:
arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0)
arm.set_motion_gains(p_trans=[float(opts.compliance), 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0)
arm.set_force_directions(['x'])
arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0])
else:
p.print_help()
return
arm.update_gains()
#! /usr/bin/python
import numpy as np
import roslib
roslib.load_manifest("hrl_pr2_arms")
import rospy
import tf
import tf.transformations as tf_trans
from std_msgs.msg import Bool, Float32
from std_srvs.srv import Empty
from geometry_msgs.msg import PoseStamped, Vector3
from hrl_generic_arms.pose_converter import PoseConverter
from hrl_pr2_arms.pr2_arm import create_pr2_arm
from hrl_pr2_arms.pr2_arm_hybrid import PR2ArmHybridForce
rospy.init_node("test_controller")
arm = create_pr2_arm("l", PR2ArmHybridForce)
arm.use_auto_update(True)
rospy.sleep(0.1)
arm.zero_sensor()
arm.reset_ep()
t, R = arm.get_ep()
arm.set_ep((t, R), 1)
q = arm.get_joint_angles()
arm.set_posture(q.tolist()[0:3] + 4 * [9999])
