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