def run_experiment(self):
self.backend.disable_interface("Setting up arm.")
pos, rot = PoseConverter.to_pos_rot(self.backend.controller.get_ell_frame())
pos += rot * np.mat([0.3, -0.2, -0.05]).T
_, tool_rot = PoseConverter.to_pos_rot([0, 0, 0], [np.pi, 0, 3./4. * np.pi])
rot *= tool_rot
ep_ac = EPArmController(self.cart_arm)
ep_ac.execute_interpolated_ep((pos, rot), 6.)
self.backend.set_arm(self.cart_arm)
self.backend.enable_interface("Controller ready.")
def run_experiment(self):
self.backend.disable_interface("Setting up arm.")
pos, rot = PoseConverter.to_pos_rot(
self.backend.controller.get_ell_frame())
pos += rot * np.mat([0.3, -0.2, -0.05]).T
_, tool_rot = PoseConverter.to_pos_rot([0, 0, 0],
[np.pi, 0, 3. / 4. * np.pi])
rot *= tool_rot
ep_ac = EPArmController(self.cart_arm)
ep_ac.execute_interpolated_ep((pos, rot), 6.)
self.backend.set_arm(self.cart_arm)
self.backend.enable_interface("Controller ready.")
def get_head_pose(self, ell_coords_rot, gripper_rot=0.):
lat, lon, height = ell_coords_rot[0]
roll, pitch, yaw = ell_coords_rot[1]
pos, rot = PoseConverter.to_pos_rot(
self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * tf_trans.euler_matrix(yaw, pitch, roll + gripper_rot,
'szyx')[:3, :3]
return pos, rot
def _ctrl_state_cb(self, ctrl_state):
with self.lock:
self.ep = PoseConverter.to_pos_rot(ctrl_state.x_desi_filtered)
with self.ctrl_state_lock:
self.ctrl_state_dict["frame"] = ctrl_state.header.frame_id
self.ctrl_state_dict["x_desi"] = PoseConverter.to_pos_rot(
ctrl_state.x_desi)
self.ctrl_state_dict["xd_desi"] = PoseConverter.to_pos_rot(
ctrl_state.xd_desi)
self.ctrl_state_dict["x_act"] = PoseConverter.to_pos_rot(
ctrl_state.x)
self.ctrl_state_dict["xd_act"] = PoseConverter.to_pos_rot(
ctrl_state.xd)
self.ctrl_state_dict["x_desi_filt"] = PoseConverter.to_pos_rot(
ctrl_state.x_desi_filtered)
self.ctrl_state_dict["x_err"] = PoseConverter.to_pos_rot(
ctrl_state.x_err)
self.ctrl_state_dict["tau_pose"] = np.array(ctrl_state.tau_pose)
self.ctrl_state_dict["tau_posture"] = np.array(
ctrl_state.tau_posture)
self.ctrl_state_dict["tau"] = np.array(ctrl_state.tau)
self.ctrl_state_dict["F"] = np.array([
ctrl_state.F.force.x, ctrl_state.F.force.y,
ctrl_state.F.force.z, ctrl_state.F.torque.x,
ctrl_state.F.torque.y, ctrl_state.F.torque.z
])
def robot_ellipsoidal_pose(self, lat, lon, height, gripper_rot, ell_frame_mat):
"""
Get pose in robot's frame of ellipsoidal coordinates
"""
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_gripper_rot = tf_trans.quaternion_from_euler(gripper_rot, 0, 0)
quat_rotated = tf_trans.quaternion_multiply(quat, quat_gripper_rot)
ell_pose_mat = PoseConverter.to_homo_mat(pos, quat_rotated)
return PoseConverter.to_pos_rot(ell_frame_mat * ell_pose_mat)
def robot_ellipsoidal_pose(self, lat, lon, height, gripper_rot,
ell_frame_mat):
"""
Get pose in robot's frame of ellipsoidal coordinates
"""
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_gripper_rot = tf_trans.quaternion_from_euler(gripper_rot, 0, 0)
quat_rotated = tf_trans.quaternion_multiply(quat, quat_gripper_rot)
ell_pose_mat = PoseConverter.to_homo_mat(pos, quat_rotated)
return PoseConverter.to_pos_rot(ell_frame_mat * ell_pose_mat)
def _ctrl_state_cb(self, ctrl_state):
with self.lock:
self.ep = PoseConverter.to_pos_rot(ctrl_state.x_desi_filtered)
with self.ctrl_state_lock:
self.ctrl_state_dict["frame"] = ctrl_state.header.frame_id
self.ctrl_state_dict["x_desi"] = PoseConverter.to_pos_rot(ctrl_state.x_desi)
self.ctrl_state_dict["xd_desi"] = PoseConverter.to_pos_rot(ctrl_state.xd_desi)
self.ctrl_state_dict["x_act"] = PoseConverter.to_pos_rot(ctrl_state.x)
self.ctrl_state_dict["xd_act"] = PoseConverter.to_pos_rot(ctrl_state.xd)
self.ctrl_state_dict["x_desi_filt"] = PoseConverter.to_pos_rot(
ctrl_state.x_desi_filtered)
self.ctrl_state_dict["x_err"] = PoseConverter.to_pos_rot(ctrl_state.x_err)
self.ctrl_state_dict["tau_pose"] = np.array(ctrl_state.tau_pose)
self.ctrl_state_dict["tau_posture"] = np.array(ctrl_state.tau_posture)
self.ctrl_state_dict["tau"] = np.array(ctrl_state.tau)
self.ctrl_state_dict["F"] = np.array([ctrl_state.F.force.x,
ctrl_state.F.force.y,
ctrl_state.F.force.z,
ctrl_state.F.torque.x,
ctrl_state.F.torque.y,
ctrl_state.F.torque.z])
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 get_head_pose(self, ell_coords_rot, gripper_rot=0.):
lat, lon, height = ell_coords_rot[0]
roll, pitch, yaw = ell_coords_rot[1]
pos, rot = PoseConverter.to_pos_rot(self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * tf_trans.euler_matrix(yaw, pitch, roll + gripper_rot, 'szyx')[:3, :3]
return pos, rot
def load_ell_params(self, e_params):
self.center, self.rot = PoseConverter.to_pos_rot(e_params.e_frame)
self.E = e_params.E
self.a = self.A * self.E
self.height = e_params.height
def load_ell_params(self, e_params):
self.center, self.rot = PoseConverter.to_pos_rot(e_params.e_frame)
self.E = e_params.E
self.a = self.A * self.E
self.height = e_params.height
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 get_head_pose(self, name, gripper_rot=0.0):
lat, lon, height = head_poses[name][0]
roll, pitch, yaw = head_poses[name][1]
pos, rot = PoseConverter.to_pos_rot(self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * tf_trans.euler_matrix(yaw, pitch, roll + gripper_rot, "rzyx")[:3, :3]
return pos, rot
