def cart_error(self, ep_actual, ep_desired): pos_act, rot_act = PoseConv.to_pos_rot(ep_actual) pos_des, rot_des = PoseConv.to_pos_rot(ep_desired) err = np.mat(np.zeros((6, 1))) err[:3,0] = pos_act - pos_des err[3:6,0] = np.mat(trans.euler_from_matrix(rot_des.T * rot_act)).T return err
def command_move_cb(self, msg): if self.arm is None: rospy.logwarn( "[cartesian_manager] Cartesian controller not enabled.") return self.cart_ctrl.stop_moving(wait=True) if msg.header.frame_id == "": msg.header.frame_id = "torso_lift_link" if self.kin is None or msg.header.frame_id not in self.kin.get_link_names( ): self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id) torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep()) torso_B_ref = self.kin.forward(base_link="torso_lift_link", end_link=msg.header.frame_id) _, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref) torso_rot_ref *= self.frame_rot ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg) change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off change_pos_xyz = change_pos.T.A[0] ep_rot_ref = torso_rot_ep.T * torso_rot_ref change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T _, change_rot_rpy = PoseConv.to_pos_euler( np.mat([0] * 3).T, change_rot) rospy.loginfo("[cartesian_manager] Command relative movement." + str((change_pos_xyz, change_rot_rpy))) self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy), ((0, 0, 0), 0), velocity=self.velocity, blocking=True)
def create_ellipsoidal_path(self, start_ell_pos, start_ell_quat, end_ell_pos, end_ell_quat, velocity=0.001, min_jerk=True): print "Start rot (%s):\r\n%s" % (type(start_ell_quat), start_ell_quat) print "End rot (%s):\r\n%s" % (type(end_ell_quat), end_ell_quat) _, start_ell_rot = PoseConv.to_pos_rot((start_ell_pos, start_ell_quat)) _, end_ell_rot = PoseConv.to_pos_rot((end_ell_pos, end_ell_quat)) rpy = trans.euler_from_matrix( start_ell_rot.T * end_ell_rot) # get roll, pitch, yaw of angle diff end_ell_pos[1] = np.mod(end_ell_pos[1], 2 * np.pi) # wrap longitude value ell_init = np.mat(start_ell_pos).T ell_final = np.mat(end_ell_pos).T # find the closest longitude angle to interpolate to if np.fabs(2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi: ell_final[1, 0] += 2 * np.pi elif np.fabs(-2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi: ell_final[1, 0] -= 2 * np.pi if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)): rospy.logerr("[ellipsoid_space] Nan values in ellipsoid. " + "ell_init: %f, %f, %f; " % (ell_init[0, 0], ell_init[1, 0], ell_init[2, 0]) + "ell_final: %f, %f, %f; " % (ell_final[0, 0], ell_final[1, 0], ell_final[2, 0])) return None num_samps = np.max([ 2, int(np.linalg.norm(ell_final - ell_init) / velocity), int(np.linalg.norm(rpy) / velocity) ]) if min_jerk: t_vals = min_jerk_traj(num_samps) else: t_vals = np.linspace(0, 1, num_samps) # smoothly interpolate from init to final ell_lat_traj = np.interp(t_vals, (0, 1), (start_ell_pos[0], end_ell_pos[0])) ell_lon_traj = np.interp(t_vals, (0, 1), (start_ell_pos[1], end_ell_pos[1])) ell_height_traj = np.interp(t_vals, (0, 1), (start_ell_pos[2], end_ell_pos[2])) ell_pos_traj = np.vstack((ell_lat_traj, ell_lon_traj, ell_height_traj)) ell_quat_traj = [ trans.quaternion_slerp(start_ell_quat, end_ell_quat, t) for t in t_vals ] return [(ell_pos_traj[:, i], ell_quat_traj[i]) for i in xrange(num_samps)]
def pose_to_ellipsoidal(self, pose): pose_pos, pose_rot = PoseConv.to_pos_rot(pose) lat, lon, height = self.pos_to_ellipsoidal(pose_pos[0, 0], pose_pos[1, 0], pose_pos[2, 0]) _, ell_rot = PoseConv.to_pos_rot( self.normal_to_ellipse(lat, lon, height)) _, quat_rot = PoseConv.to_pos_quat( np.mat([0] * 3).T, ell_rot.T * pose_rot) return [lat, lon, height], quat_rot
def inverse(self, pose, q_guess=None, min_joints=None, max_joints=None): pos, rot = PoseConv.to_pos_rot(pose) pos_kdl = kdl.Vector(pos[0,0], pos[1,0], pos[2,0]) rot_kdl = kdl.Rotation(rot[0,0], rot[0,1], rot[0,2], rot[1,0], rot[1,1], rot[1,2], rot[2,0], rot[2,1], rot[2,2]) frame_kdl = kdl.Frame(rot_kdl, pos_kdl) if min_joints is None: min_joints = self.joint_safety_lower if max_joints is None: max_joints = self.joint_safety_upper mins_kdl = joint_list_to_kdl(min_joints) maxs_kdl = joint_list_to_kdl(max_joints) ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(self.chain, mins_kdl, maxs_kdl, self._fk_kdl, self._ik_v_kdl) if q_guess == None: # use the midpoint of the joint limits as the guess lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.) upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.) q_guess = (lower_lim + upper_lim) / 2.0 q_guess = np.where(np.isnan(q_guess), [0.]*len(q_guess), q_guess) q_kdl = kdl.JntArray(self.num_joints) q_guess_kdl = joint_list_to_kdl(q_guess) if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0: return np.array(joint_kdl_to_list(q_kdl)) else: return None
def command_absolute_cb(self, msg): if self.arm is None: rospy.logwarn( "[cartesian_manager] Cartesian controller not enabled.") return self.cart_ctrl.stop_moving(wait=True) if msg.header.frame_id == "": msg.header.frame_id = "torso_lift_link" if self.kin is None or msg.header.frame_id not in self.kin.get_link_names( ): self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id) torso_pos_ep, torso_rot_ep = self.arm.get_ep() torso_B_ref = self.kin.forward(base_link="torso_lift_link", end_link=msg.header.frame_id) ref_B_goal = PoseConv.to_homo_mat(msg) torso_B_goal = torso_B_ref * ref_B_goal change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal) change_pos_xyz = change_pos.T.A[0] rospy.loginfo("[cartesian_manager] Command absolute movement." + str((change_pos_xyz, change_rot))) self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot), ((1, 1, 1), 1), velocity=self.velocity, blocking=True)
def inverse(self, pose, q_guess=None, min_joints=None, max_joints=None): pos, rot = PoseConv.to_pos_rot(pose) pos_kdl = kdl.Vector(pos[0, 0], pos[1, 0], pos[2, 0]) rot_kdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0], rot[1, 1], rot[1, 2], rot[2, 0], rot[2, 1], rot[2, 2]) frame_kdl = kdl.Frame(rot_kdl, pos_kdl) if min_joints is None: min_joints = self.joint_safety_lower if max_joints is None: max_joints = self.joint_safety_upper mins_kdl = joint_list_to_kdl(min_joints) maxs_kdl = joint_list_to_kdl(max_joints) ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(self.chain, mins_kdl, maxs_kdl, self._fk_kdl, self._ik_v_kdl) if q_guess == None: # use the midpoint of the joint limits as the guess lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.) upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.) q_guess = (lower_lim + upper_lim) / 2.0 q_guess = np.where(np.isnan(q_guess), [0.] * len(q_guess), q_guess) q_kdl = kdl.JntArray(self.num_joints) q_guess_kdl = joint_list_to_kdl(q_guess) if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0: return np.array(joint_kdl_to_list(q_kdl)) else: return None
def robot_ellipsoidal_pose(self, lat, lon, height, orient_quat, kinect_frame_mat=None): if kinect_frame_mat is None: kinect_frame_mat = self.get_ell_frame() pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height) quat_rotated = trans.quaternion_multiply(quat, orient_quat) ell_pose_mat = PoseConv.to_homo_mat(pos, quat_rotated) return PoseConv.to_pos_rot(kinect_frame_mat * ell_pose_mat)
def inverse_biased(self, pose, q_init, q_bias, q_bias_weights, rot_weight=1., bias_vel=0.01, num_iter=100): # This code is potentially volitile q_out = np.mat(self.inverse_search(pose)).T for i in range(num_iter): pos_fk, rot_fk = PoseConv.to_pos_rot(self.forward(q_out)) delta_twist = np.mat(np.zeros((6, 1))) pos_delta = pos - pos_fk delta_twist[:3, 0] = pos_delta rot_delta = np.mat(np.eye(4)) rot_delta[:3, :3] = rot * rot_fk.T rot_delta_angles = np.mat(trans.euler_from_matrix(rot_delta)).T delta_twist[3:6, 0] = rot_delta_angles J = self.jacobian(q_out) J[3:6, :] *= np.sqrt(rot_weight) delta_twist[3:6, 0] *= np.sqrt(rot_weight) J_tinv = np.linalg.inv(J.T * J + np.diag(q_bias_weights) * np.eye(len(q_init))) * J.T q_bias_diff = q_bias - q_out q_bias_diff_normed = q_bias_diff * bias_vel / np.linalg.norm( q_bias_diff) delta_q = q_bias_diff_normed + J_tinv * (delta_twist - J * q_bias_diff_normed) q_out += delta_q q_out = np.mat(self.clip_joints_safe(q_out.T.A[0])).T return q_out
def get_head_pose(self, name, gripper_rot=0.): lat, lon, height = head_poses[name][0] roll, pitch, yaw = head_poses[name][1] pos, rot = PoseConv.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
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 = PoseConv.to_pos_rot( self.ell_space.ellipsoidal_to_pose(lat, lon, height)) rot = rot * trans.euler_matrix(yaw, pitch, roll + gripper_rot, 'szyx')[:3, :3] return pos, rot
def FK(self, q, link_number=None): if link_number is not None: end_link = self.get_link_names(fixed=False)[link_number] else: end_link = None homo_mat = self.forward(q, end_link) pos, rot = PoseConv.to_pos_rot(homo_mat) return pos, rot
def enable_controller_cb(req): if req.enable: _, frame_rot = PoseConv.to_pos_rot([0]*3, [req.frame_rot.x, req.frame_rot.y, req.frame_rot.z]) if req.velocity == 0: req.velocity = 0.03 success = self.enable_controller(req.end_link, req.ctrl_params, req.ctrl_name, frame_rot, velocity=req.velocity) else: success = self.disable_controller() return EnableCartControllerResponse(success)
def _ctrl_state_cb(self, ctrl_state): self._save_ep(PoseConv.to_homo_mat(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"] = PoseConv.to_pos_rot(ctrl_state.x_desi) self.ctrl_state_dict["xd_desi"] = extract_twist(ctrl_state.xd_desi) self.ctrl_state_dict["x_act"] = PoseConv.to_pos_rot(ctrl_state.x) self.ctrl_state_dict["xd_act"] = extract_twist(ctrl_state.xd) self.ctrl_state_dict["x_desi_filt"] = PoseConv.to_pos_rot( ctrl_state.x_desi_filtered) self.ctrl_state_dict["x_err"] = extract_twist(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 execute_cart_move(self, change_ep, abs_sel, velocity=0.001, num_samps=None, blocking=True): if not self.cmd_lock.acquire(False): return False cur_pos, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep()) change_pos_ep, change_rot_ep = change_ep abs_cart_ep_sel, is_abs_rot = abs_sel pos_f = np.where(abs_cart_ep_sel, change_pos_ep, np.array(cur_pos + cur_rot * np.mat(change_pos_ep).T).T[0]) if is_abs_rot: rot_mat_f = change_rot_ep else: rpy = change_rot_ep _, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep()) rot_mat = np.mat(trans.euler_matrix(*rpy))[:3,:3] rot_mat_f = cur_rot * rot_mat traj = self._create_cart_trajectory(pos_f, rot_mat_f, velocity, num_samps) retval = self._run_traj(traj, blocking=blocking) self.cmd_lock.release() return retval
def create_tool_arrow(): arrows = MarkerArray() color = ColorRGBA(0., 0., 1., 1.) for i, param in enumerate(params): ell_pos, ell_rot = params[param] _, ell_rot_mat = PoseConv.to_pos_rot([0]*3, ell_rot) cart_pose = PoseConv.to_homo_mat(ell_space.ellipsoidal_to_pose(*ell_pos)) cart_pose[:3,:3] = cart_pose[:3,:3] * ell_rot_mat arrow = create_arrow_marker(cart_pose, i, color) arrow.header.stamp = rospy.Time.now() arrows.markers.append(arrow) return arrows
def create_ellipsoidal_path(self, start_ell_pos, start_ell_quat, end_ell_pos, end_ell_quat, velocity=0.001, min_jerk=True): print "Start rot (%s):\r\n%s" %(type(start_ell_quat),start_ell_quat) print "End rot (%s):\r\n%s" %(type(end_ell_quat),end_ell_quat) _, start_ell_rot = PoseConv.to_pos_rot((start_ell_pos,start_ell_quat)) _, end_ell_rot = PoseConv.to_pos_rot((end_ell_pos,end_ell_quat)) rpy = trans.euler_from_matrix(start_ell_rot.T * end_ell_rot) # get roll, pitch, yaw of angle diff end_ell_pos[1] = np.mod(end_ell_pos[1], 2 * np.pi) # wrap longitude value ell_init = np.mat(start_ell_pos).T ell_final = np.mat(end_ell_pos).T # find the closest longitude angle to interpolate to if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi: ell_final[1,0] += 2 * np.pi elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi: ell_final[1,0] -= 2 * np.pi if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)): rospy.logerr("[ellipsoid_space] Nan values in ellipsoid. " + "ell_init: %f, %f, %f; " % (ell_init[0,0], ell_init[1,0], ell_init[2,0]) + "ell_final: %f, %f, %f; " % (ell_final[0,0], ell_final[1,0], ell_final[2,0])) return None num_samps = np.max([2, int(np.linalg.norm(ell_final - ell_init) / velocity), int(np.linalg.norm(rpy) / velocity)]) if min_jerk: t_vals = min_jerk_traj(num_samps) else: t_vals = np.linspace(0,1,num_samps) # smoothly interpolate from init to final ell_lat_traj = np.interp(t_vals, (0,1),(start_ell_pos[0], end_ell_pos[0])) ell_lon_traj = np.interp(t_vals, (0,1),(start_ell_pos[1], end_ell_pos[1])) ell_height_traj = np.interp(t_vals, (0,1),(start_ell_pos[2], end_ell_pos[2])) ell_pos_traj = np.vstack((ell_lat_traj, ell_lon_traj, ell_height_traj)) ell_quat_traj = [trans.quaternion_slerp(start_ell_quat, end_ell_quat, t) for t in t_vals] return [(ell_pos_traj[:,i], ell_quat_traj[i]) for i in xrange(num_samps)]
def _create_cart_trajectory(self, pos_f, rot_mat_f, velocity=0.001, num_samps=None): cur_pos, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep()) rpy = trans.euler_from_matrix(cur_rot.T * rot_mat_f) # get roll, pitch, yaw of angle diff if num_samps is None: num_samps = np.max([2, int(np.linalg.norm(pos_f - cur_pos) / velocity), int(np.linalg.norm(rpy) / velocity)]) traj = self.arm.interpolate_ep([cur_pos, cur_rot], [np.mat(pos_f).T, rot_mat_f], min_jerk_traj(num_samps)) return traj
def command_move_cb(self, msg): if self.arm is None: rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.") return self.cart_ctrl.stop_moving(wait=True) if msg.header.frame_id == "": msg.header.frame_id = "torso_lift_link" if self.kin is None or msg.header.frame_id not in self.kin.get_link_names(): self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id) torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep()) torso_B_ref = self.kin.forward(base_link="torso_lift_link", end_link=msg.header.frame_id) _, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref) torso_rot_ref *= self.frame_rot ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg) change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off change_pos_xyz = change_pos.T.A[0] ep_rot_ref = torso_rot_ep.T * torso_rot_ref change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T _, change_rot_rpy = PoseConv.to_pos_euler(np.mat([0]*3).T, change_rot) rospy.loginfo("[cartesian_manager] Command relative movement." + str((change_pos_xyz, change_rot_rpy))) self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy), ((0, 0, 0), 0), velocity=self.velocity, blocking=True)
def enable_controller_cb(req): if req.enable: _, frame_rot = PoseConv.to_pos_rot( [0] * 3, [req.frame_rot.x, req.frame_rot.y, req.frame_rot.z]) if req.velocity == 0: req.velocity = 0.03 success = self.enable_controller(req.end_link, req.ctrl_params, req.ctrl_name, frame_rot, velocity=req.velocity) else: success = self.disable_controller() return EnableCartControllerResponse(success)
def _create_ell_trajectory(self, ell_f, rot_mat_f, orient_quat=[0., 0., 0., 1.], velocity=0.001): _, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep()) rpy = trans.euler_from_matrix(cur_rot.T * rot_mat_f) # get roll, pitch, yaw of angle diff ell_f[1] = np.mod(ell_f[1], 2 * np.pi) # wrap longitude value # get the current ellipsoidal location of the end effector ell_init = np.mat(self.get_ell_ep()).T ell_final = np.mat(ell_f).T # find the closest longitude angle to interpolate to if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi: ell_final[1,0] += 2 * np.pi elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi: ell_final[1,0] -= 2 * np.pi if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)): rospy.logerr("[ellipsoid_controller] Nan values in ellipsoid EPs. " + "ell_init: %f, %f, %f; " % (ell_init[0,0], ell_init[1,0], ell_init[2,0]) + "ell_final: %f, %f, %f; " % (ell_final[0,0], ell_final[1,0], ell_final[2,0])) return None num_samps = np.max([2, int(np.linalg.norm(ell_final - ell_init) / velocity), int(np.linalg.norm(rpy) / velocity)]) t_vals = min_jerk_traj(num_samps) # makes movement smooth # smoothly interpolate from init to final ell_traj = np.array(ell_init) + np.array(np.tile(ell_final - ell_init, (1, num_samps))) * np.array(t_vals) ell_frame_mat = self.ell_server.get_ell_frame() ell_pose_traj = [self.ell_server.robot_ellipsoidal_pose( ell_traj[0,i], ell_traj[1,i], ell_traj[2,i], orient_quat, ell_frame_mat) for i in range(ell_traj.shape[1])] # modify rotation of trajectory _, ell_init_rot = self.ell_server.robot_ellipsoidal_pose( ell_init[0,0], ell_init[1,0], ell_init[2,0], orient_quat, ell_frame_mat) rot_adjust_traj = self.arm.interpolate_ep([np.mat([0]*3).T, cur_rot], [np.mat([0]*3).T, rot_mat_f], min_jerk_traj(num_samps)) ell_pose_traj = [(ell_pose_traj[i][0], ell_pose_traj[i][1] * ell_init_rot.T * rot_adjust_traj[i][1]) for i in range(num_samps)] return ell_pose_traj
def _run_traj(self, traj, blocking=True): self.start_pub.publish( PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[0])) self.end_pub.publish( PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[-1])) # make sure traj beginning is close to current end effector position init_pos_tolerance = rospy.get_param("~init_pos_tolerance", 0.05) init_rot_tolerance = rospy.get_param("~init_rot_tolerance", np.pi/12) ee_pos, ee_rot = PoseConv.to_pos_rot(self.arm.get_end_effector_pose()) _, rot_diff = PoseConv.to_pos_euler((ee_pos, ee_rot * traj[0][1].T)) pos_diff = np.linalg.norm(ee_pos - traj[0][0]) if pos_diff > init_pos_tolerance: rospy.logwarn("[controller_base] End effector too far from current position. " + "Pos diff: %.3f, Tolerance: %.3f" % (pos_diff, init_pos_tolerance)) return False if np.linalg.norm(rot_diff) > init_rot_tolerance: rospy.logwarn("[controller_base] End effector too far from current rotation. " + "Rot diff: %.3f, Tolerance: %.3f" % (np.linalg.norm(rot_diff), init_rot_tolerance)) return False return self.execute_cart_traj(self.arm, traj, self.time_step, blocking=blocking)
def command_absolute_cb(self, msg): if self.arm is None: rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.") return self.cart_ctrl.stop_moving(wait=True) if msg.header.frame_id == "": msg.header.frame_id = "torso_lift_link" if self.kin is None or msg.header.frame_id not in self.kin.get_link_names(): self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id) torso_pos_ep, torso_rot_ep = self.arm.get_ep() torso_B_ref = self.kin.forward(base_link="torso_lift_link", end_link=msg.header.frame_id) ref_B_goal = PoseConv.to_homo_mat(msg) torso_B_goal = torso_B_ref * ref_B_goal change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal) change_pos_xyz = change_pos.T.A[0] rospy.loginfo("[cartesian_manager] Command absolute movement." + str((change_pos_xyz, change_rot))) self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot), ((1, 1, 1), 1), velocity=self.velocity, blocking=True)
def inverse_biased(self, pose, q_init, q_bias, q_bias_weights, rot_weight=1.0, bias_vel=0.01, num_iter=100): # This code is potentially volitile q_out = np.mat(self.inverse_search(pose)).T for i in range(num_iter): pos_fk, rot_fk = PoseConv.to_pos_rot(self.forward(q_out)) delta_twist = np.mat(np.zeros((6, 1))) pos_delta = pos - pos_fk delta_twist[:3, 0] = pos_delta rot_delta = np.mat(np.eye(4)) rot_delta[:3, :3] = rot * rot_fk.T rot_delta_angles = np.mat(trans.euler_from_matrix(rot_delta)).T delta_twist[3:6, 0] = rot_delta_angles J = self.jacobian(q_out) J[3:6, :] *= np.sqrt(rot_weight) delta_twist[3:6, 0] *= np.sqrt(rot_weight) J_tinv = np.linalg.inv(J.T * J + np.diag(q_bias_weights) * np.eye(len(q_init))) * J.T q_bias_diff = q_bias - q_out q_bias_diff_normed = q_bias_diff * bias_vel / np.linalg.norm(q_bias_diff) delta_q = q_bias_diff_normed + J_tinv * (delta_twist - J * q_bias_diff_normed) q_out += delta_q q_out = np.mat(self.clip_joints_safe(q_out.T.A[0])).T return q_out
def _parse_ell_move(self, change_ep, abs_sel, orient_quat): #change_ell_ep = [lat, lon, height], change_rot_ep = Rotation in matrix or quat form (3x3 rotation mat, 1x4 quat) #Provides new position and orientation goals (in relative or abs manner, determined next). change_ell_ep, change_rot_ep = change_ep #abs_ell_ep_sel = 1x3 list of 0/1, is_abs_rot = 0/1 (Bool) #abs_ell_ep_sel: selects lat, long, height absolute vs relative position command #change_rot_ep: selects abs vs relative use of orientation. abs_ell_ep_sel, is_abs_rot = abs_sel ell_f = np.where(abs_ell_ep_sel, change_ell_ep, np.array(self.get_ell_ep()) + np.array(change_ell_ep)) print "old", ell_f if ell_f[0] > np.pi: ell_f[0] = 2 * np.pi - ell_f[0] ell_f[1] *= -1 if ell_f[0] < 0.: ell_f[0] *= -1 ell_f[1] *= -1 ell_f[1] = np.mod(ell_f[1], 2 * np.pi) ell_f = self._clip_ell_ep(ell_f) print "new", ell_f if is_abs_rot: #Apply rotation from change_rot_ep as final orientation of pose #Expects rotation matrix in change_rot_ep rot_change_mat = change_rot_ep _, ell_final_rot = self.ell_server.robot_ellipsoidal_pose(ell_f[0], ell_f[1], ell_f[2], orient_quat) rot_mat_f = ell_final_rot * rot_change_mat else: #Apply rotation from change_rot_ep as change to current rotation #Expects quaternion in change_rot_ep quat = change_rot_ep _, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep()) rot_mat = np.mat(trans.quaternion_matrix(quat))[:3,:3] rot_mat_f = cur_rot * rot_mat return ell_f, rot_mat_f
def _parse_ell_move(self, change_ep, abs_sel, orient_quat): #change_ell_ep = [lat, lon, height], change_rot_ep = Rotation in matrix or quat form (3x3 rotation mat, 1x4 quat) #Provides new position and orientation goals (in relative or abs manner, determined next). change_ell_ep, change_rot_ep = change_ep #abs_ell_ep_sel = 1x3 list of 0/1, is_abs_rot = 0/1 (Bool) #abs_ell_ep_sel: selects lat, long, height absolute vs relative position command #change_rot_ep: selects abs vs relative use of orientation. abs_ell_ep_sel, is_abs_rot = abs_sel ell_f = np.where(abs_ell_ep_sel, change_ell_ep, np.array(self.get_ell_ep()) + np.array(change_ell_ep)) print "old", ell_f if ell_f[0] > np.pi: ell_f[0] = 2 * np.pi - ell_f[0] ell_f[1] *= -1 if ell_f[0] < 0.: ell_f[0] *= -1 ell_f[1] *= -1 ell_f[1] = np.mod(ell_f[1], 2 * np.pi) ell_f = self._clip_ell_ep(ell_f) print "new", ell_f if is_abs_rot: #Apply rotation from change_rot_ep as final orientation of pose #Expects rotation matrix in change_rot_ep rot_change_mat = change_rot_ep _, ell_final_rot = self.ell_server.robot_ellipsoidal_pose( ell_f[0], ell_f[1], ell_f[2], orient_quat) rot_mat_f = ell_final_rot * rot_change_mat else: #Apply rotation from change_rot_ep as change to current rotation #Expects quaternion in change_rot_ep quat = change_rot_ep _, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep()) rot_mat = np.mat(trans.quaternion_matrix(quat))[:3, :3] rot_mat_f = cur_rot * rot_mat return ell_f, rot_mat_f
roslib.load_manifest("hrl_face_adls") import rospy from std_msgs.msg import Bool, Float32 from geometry_msgs.msg import TwistStamped, PoseStamped from hrl_ellipsoidal_control.controller_base import CartesianStepController from pykdl_utils.joint_kinematics import create_joint_kin from hrl_geom.pose_converter import PoseConv from hrl_pr2_arms.pr2_arm_jt_task import create_ep_arm, PR2ArmJTransposeTask from hrl_pr2_arms.pr2_controller_switcher import ControllerSwitcher from face_adls_manager import async_call from hrl_face_adls.srv import EnableCartController, EnableCartControllerResponse VELOCITY = 0.03 _, FLIP_PERSPECTIVE_ROT = PoseConv.to_pos_rot([0] * 3, [0, 0, np.pi]) class CartesianControllerManager(object): def __init__(self, arm_char): self.arm_char = arm_char self.arm = None self.kin = None self.cart_ctrl = CartesianStepController() self.ctrl_switcher = ControllerSwitcher() self.command_move_sub = rospy.Subscriber( "/face_adls/%s_cart_move" % arm_char, TwistStamped, async_call(self.command_move_cb)) self.command_absolute_sub = rospy.Subscriber( "/face_adls/%s_cart_absolute" % arm_char, PoseStamped, async_call(self.command_absolute_cb))
#!/usr/bin/python import numpy as np import roslib roslib.load_manifest("hrl_geom") import rospy from hrl_geom.pose_converter import PoseConv if __name__ == "__main__": rospy.init_node("test_poseconv") homo_mat = PoseConv.to_homo_mat([0., 1., 2.], [0., 0., np.pi/2]) pose_msg = PoseConv.to_pose_msg(homo_mat) pos, quat = PoseConv.to_pos_quat(pose_msg) pose_stamped_msg = PoseConv.to_pose_stamped_msg("/base_link", pos, quat) pose_stamped_msg2 = PoseConv.to_pose_stamped_msg("/base_link", [pos, quat]) tf_stamped = PoseConv.to_tf_stamped_msg("/new_link", pose_stamped_msg) p, R = PoseConv.to_pos_rot("/new_link", tf_stamped) for name in ["homo_mat", "pose_msg", "pos", "quat", "pose_stamped_msg", "pose_stamped_msg2", "tf_stamped", "p", "R"]: print "%s: \n" % name, locals()[name]
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 = PoseConv.to_pos_rot(self.ell_space.ellipsoidal_to_pose(lat, lon, height)) rot = rot * trans.euler_matrix(yaw, pitch, roll + gripper_rot, 'szyx')[:3, :3] return pos, rot
def link_trans(): pub = rospy.Publisher('coodinator_msgs_to_gui', String, queue_size=1000) rospy.init_node('link_trans_gp8', anonymous=True) rate = rospy.Rate(100) rospy.sleep(1) rospack = rospkg.RosPack() rospack.list() # Read in robot description from parameter srv # and build kinematic model robot = URDF.from_parameter_server() base_link = robot.get_root() end_link = robot.link_map.keys()[len(robot.link_map) - 1] print "Root link: %s; End link: %s" % (base_link, end_link) kdl_kin = KDLKinematics(robot, base_link, end_link) print "Robot ik model built...\n" print len(kdl_kin.get_link_names()) # link_files = [] for i in range(8): link_files.append( rospack.get_path('motoman_app') + "/joint_data/link_" + str(i) + "_data.csv") print link_files[i] joint_filename = rospack.get_path( 'motoman_app') + "/joint_data/joint_value_gp8_data.csv" print "joint_filename" home_pose_file = rospack.get_path( 'motoman_app') + "/joint_data/home_pose.csv" in_value = np.genfromtxt(joint_filename, delimiter=',') # n by 12 array print "Data stored" (row, col) = in_value.shape print "Data length: ", row # q_init = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) tf2 = np.array([]) for i in range(8): pose = kdl_kin.forward(q_init, kdl_kin.get_link_names()[i], kdl_kin.get_link_names()[0]) pos, rot = PoseConv.to_pos_rot(pose) tf_temp = np.array([ pos[0, 0], pos[1, 0], pos[2, 0], rot[0, 0], rot[1, 0], rot[2, 0], rot[0, 1], rot[1, 1], rot[2, 1], rot[0, 2], rot[1, 2], rot[2, 2] ]) if len(tf2) == 0: tf2 = tf_temp else: tf2 = np.vstack([tf2, tf_temp]) print "home_pose done" np.savetxt(home_pose_file, tf2, fmt='%f', delimiter=",") for i in range(8): tf = np.array([]) for j in range(row): q = in_value[j] # pose=kdl_kin._do_kdl_fk(q, i) pose = kdl_kin.forward(q, kdl_kin.get_link_names()[i], kdl_kin.get_link_names()[0]) pos, rot = PoseConv.to_pos_rot(pose) tf_temp = np.array([ pos[0, 0], pos[1, 0], pos[2, 0], rot[0, 0], rot[1, 0], rot[2, 0], rot[0, 1], rot[1, 1], rot[2, 1], rot[0, 2], rot[1, 2], rot[2, 2] ]) if j == 0: tf = tf_temp else: tf = np.vstack([tf, tf_temp]) print "link", i, " done" (length, width) = tf.shape np.savetxt(link_files[i], tf, fmt='%f', delimiter=",") print "link", i, "saved" result_str = "link_saved" pub.publish(result_str)
def pose_to_ellipsoidal(self, pose): pose_pos, pose_rot = PoseConv.to_pos_rot(pose) lat, lon, height = self.pos_to_ellipsoidal(pose_pos[0,0], pose_pos[1,0], pose_pos[2,0]) _, ell_rot = PoseConv.to_pos_rot(self.normal_to_ellipse(lat, lon, height)) _, quat_rot = PoseConv.to_pos_quat(np.mat([0]*3).T, ell_rot.T * pose_rot) return [lat, lon, height], quat_rot
def _create_ell_trajectory(self, ell_f, rot_mat_f, orient_quat=[0., 0., 0., 1.], velocity=0.001): _, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep()) rpy = trans.euler_from_matrix( cur_rot.T * rot_mat_f) # get roll, pitch, yaw of angle diff ell_f[1] = np.mod(ell_f[1], 2 * np.pi) # wrap longitude value # get the current ellipsoidal location of the end effector ell_init = np.mat(self.get_ell_ep()).T ell_final = np.mat(ell_f).T # find the closest longitude angle to interpolate to if np.fabs(2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi: ell_final[1, 0] += 2 * np.pi elif np.fabs(-2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi: ell_final[1, 0] -= 2 * np.pi if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)): rospy.logerr( "[ellipsoid_controller] Nan values in ellipsoid EPs. " + "ell_init: %f, %f, %f; " % (ell_init[0, 0], ell_init[1, 0], ell_init[2, 0]) + "ell_final: %f, %f, %f; " % (ell_final[0, 0], ell_final[1, 0], ell_final[2, 0])) return None num_samps = np.max([ 2, int(np.linalg.norm(ell_final - ell_init) / velocity), int(np.linalg.norm(rpy) / velocity) ]) t_vals = min_jerk_traj(num_samps) # makes movement smooth # smoothly interpolate from init to final ell_traj = np.array(ell_init) + np.array( np.tile(ell_final - ell_init, (1, num_samps))) * np.array(t_vals) ell_frame_mat = self.ell_server.get_ell_frame() ell_pose_traj = [ self.ell_server.robot_ellipsoidal_pose(ell_traj[0, i], ell_traj[1, i], ell_traj[2, i], orient_quat, ell_frame_mat) for i in range(ell_traj.shape[1]) ] # modify rotation of trajectory _, ell_init_rot = self.ell_server.robot_ellipsoidal_pose( ell_init[0, 0], ell_init[1, 0], ell_init[2, 0], orient_quat, ell_frame_mat) rot_adjust_traj = self.arm.interpolate_ep( [np.mat([0] * 3).T, cur_rot], [np.mat([0] * 3).T, rot_mat_f], min_jerk_traj(num_samps)) ell_pose_traj = [ (ell_pose_traj[i][0], ell_pose_traj[i][1] * ell_init_rot.T * rot_adjust_traj[i][1]) for i in range(num_samps) ] return ell_pose_traj
roslib.load_manifest("hrl_face_adls") import rospy from std_msgs.msg import Bool, Float32 from geometry_msgs.msg import TwistStamped, PoseStamped from hrl_ellipsoidal_control.controller_base import CartesianStepController from pykdl_utils.joint_kinematics import create_joint_kin from hrl_geom.pose_converter import PoseConv from hrl_pr2_arms.pr2_arm_jt_task import create_ep_arm, PR2ArmJTransposeTask from hrl_pr2_arms.pr2_controller_switcher import ControllerSwitcher from face_adls_manager import async_call from hrl_face_adls.srv import EnableCartController, EnableCartControllerResponse VELOCITY = 0.03 _, FLIP_PERSPECTIVE_ROT = PoseConv.to_pos_rot([0]*3, [0, 0, np.pi]) class CartesianControllerManager(object): def __init__(self, arm_char): self.arm_char = arm_char self.arm = None self.kin = None self.cart_ctrl = CartesianStepController() self.ctrl_switcher = ControllerSwitcher() self.command_move_sub = rospy.Subscriber("/face_adls/%s_cart_move" % arm_char, TwistStamped, async_call(self.command_move_cb)) self.command_absolute_sub = rospy.Subscriber("/face_adls/%s_cart_absolute" % arm_char, PoseStamped, async_call(self.command_absolute_cb)) self.adjust_posture_sub = rospy.Subscriber("/face_adls/%s_cart_posture" % arm_char, Float32, self.adjust_posture_cb) def enable_controller_cb(req):