def laplace_cost_and_grad_pose(self, theta, mu_theta, inv_sigma_theta, mu_x, inv_sigma_x, mu_ang_euler_des, inv_sig_euler): f_th, T_joint = self.fwd_kin(theta) jac_th = self.geometric_jacobian(T_joint, f_th) euler_angles = tf_tran.euler_from_matrix(f_th, 'szyz') s_phi, s_nu, c_phi, c_nu = np.sin(euler_angles[0]), np.sin( euler_angles[1]), np.cos(euler_angles[0]), np.cos(euler_angles[1]) T = np.array([[0, -s_phi, c_phi * s_nu], [0, c_phi, s_phi * s_nu], [1, 0, c_nu]]) jac_analytic = np.dot(tf_tran.inverse_matrix(T), jac_th[3:6, :]) pos_th = f_th[0:3, 3] jac_pos_th = jac_th[0:3, :] diff1 = theta - mu_theta tmp1 = np.dot(inv_sigma_theta, diff1) diff2 = pos_th - mu_x tmp2 = np.dot(inv_sigma_x, diff2) ori_th = tf_tran.euler_from_matrix(f_th, 'szyz') jac_ori_th = jac_analytic # [3:6, :] diff3 = np.array(ori_th) - np.array(mu_ang_euler_des) tmp3 = np.dot(inv_sig_euler, diff3) nll = 0.5 * (np.dot(diff1, tmp1) + np.dot(diff2, tmp2)) + 0.5 * np.dot( diff3, tmp3) # print 'nll', nll # print '###########################' grad_nll = tmp1 + np.dot(jac_pos_th.T, tmp2) + np.dot( jac_ori_th.T, tmp3) return nll, grad_nll
def main(): rospack = rospkg.RosPack() pkg_path = rospack.get_path('hrl_base_selection') bag_path = '/home/ari/svn/robot1_data/2011_ijrr_phri/log_data/bag/' #bag_name = 'sub1_shaver' #bag_name = 'sub3_shaver' bag_name = 'sub6_shaver' adl2_B_toolframe = createBMatrix([0.234, 0.041, -0.015], [0, 0, 0, 1]) toolframe_B_shaver = createBMatrix([0.070, 0., 0.], [0, 0, 0, 1]) shaver_B_pr2goal = createBMatrix([-0.050, 0., 0.], [0, 0, 0, 1]) adl2_B_pr2goal = adl2_B_toolframe*toolframe_B_shaver*shaver_B_pr2goal head_B_headc = createBMatrix([0.21, 0.02, -0.087], [0, 0, 0, 1]) print 'Starting on the tool log file! \n' bag = rosbag.Bag(''.join([bag_path,bag_name,'.bag']), 'r') tool = open(''.join([pkg_path,'/data/',bag_name,'_tool.log']), 'w') for topic, tf_msg, t in bag.read_messages(): if topic == "/tf": if len(tf_msg.transforms) > 0 and tf_msg.transforms[0].child_frame_id == "/adl2": tx = copy.copy(tf_msg.transforms[0].transform.translation.x) ty = copy.copy(tf_msg.transforms[0].transform.translation.y) tz = copy.copy(tf_msg.transforms[0].transform.translation.z) rx = copy.copy(tf_msg.transforms[0].transform.rotation.x) ry = copy.copy(tf_msg.transforms[0].transform.rotation.y) rz = copy.copy(tf_msg.transforms[0].transform.rotation.z) rw = copy.copy(tf_msg.transforms[0].transform.rotation.w) world_B_shaver = createBMatrix([tx,ty,tz],[rx,ry,rz,rw])*adl2_B_pr2goal eul = tft.euler_from_matrix(world_B_shaver,'rxyz') #print world_B_shaver,'\n' #print eul,'\n' #print 'time: \n',t tool.write(''.join([str(t),' %f %f %f %f %f %f \n' % (world_B_shaver[0,3],world_B_shaver[1,3],world_B_shaver[2,3],eul[0],eul[1],eul[2])])) #tool.write(''.join([world_B_shaver[0,3],' ',world_B_shaver[1,3],' ',world_B_shaver[2,3],' ',eul[0],' ',eul[1],' ',eul[2],'\n'])) bag.close() tool.close() print 'Starting on the head log file! \n' bag = rosbag.Bag(''.join([bag_path,bag_name,'.bag']), 'r') head = open(''.join([pkg_path,'/data/',bag_name,'_head.log']), 'w') for topic, tf_msg, t in bag.read_messages(): if topic == "/tf": if len(tf_msg.transforms) > 0 and tf_msg.transforms[0].child_frame_id == "/head": tx = copy.copy(tf_msg.transforms[0].transform.translation.x) ty = copy.copy(tf_msg.transforms[0].transform.translation.y) tz = copy.copy(tf_msg.transforms[0].transform.translation.z) rx = copy.copy(tf_msg.transforms[0].transform.rotation.x) ry = copy.copy(tf_msg.transforms[0].transform.rotation.y) rz = copy.copy(tf_msg.transforms[0].transform.rotation.z) rw = copy.copy(tf_msg.transforms[0].transform.rotation.w) world_B_headc = createBMatrix([tx,ty,tz],[rx,ry,rz,rw])*head_B_headc eul = copy.copy(tft.euler_from_matrix(world_B_headc,'rxyz')) head.write(''.join([str(t),' %f %f %f %f %f %f \n' % (world_B_headc[0,3],world_B_headc[1,3],world_B_headc[2,3],eul[0],eul[1],eul[2])])) #head.write(''.join([t,' ',world_B_head[0,3],' ',world_B_head[1,3],' ',world_B_head[2,3],' ',eul[0],' ',eul[1],' ',eul[2],' ',eul[3],'\n'])) bag.close() head.close() print "Saved files!"
def moveItTo2(self, destin): if self.shouldMove == False: self.destin_list2.insert(0, destin) return delay = rospy.Rate(1.0) if len(self.destin_list2) > 0: self.destin_list2.insert(0, destin) destin = self.destin_list2.pop() while not rospy.is_shutdown(): try: (translation, orientation) = listener.lookupTransform( "/odom", "/base_footprint", rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: print("EXCEPTION:", e) delay.sleep() print("Robot is believed to be at (x,y,z): (", translation[0], ",", translation[1], ",", translation[2], ")") r_x, r_y, r_z = transformations.euler_from_matrix( transformations.quaternion_matrix(orientation)) print("Robot's angle with the Z-axis is: ", r_z) print("Robot's angle with the Y-axis is: ", r_y) print("Robot's angle with the X-axis is: ", r_x) print("Destination is believed to be at (x,y,z): (", destin.translation.x, ",", destin.translation.y, ",", destin.translation.z, ")") destinrotq = [ destin.rotation.x, destin.rotation.y, destin.rotation.z, destin.rotation.w ] d_x, d_y, d_z = transformations.euler_from_matrix( transformations.quaternion_matrix(destinrotq)) print("Destination's angle with the Z-axis is: ", d_z) print("Destination's angle with the Y-axis is: ", d_y) print("Destination's angle with the X-axis is: ", d_x) motor_command = Twist() x = destin.translation.x - translation[0] y = destin.translation.y - translation[1] z = destin.translation.z - translation[2] theta1 = M.atan2(y, x) theta2 = M.atan2(y, x) newSpeed = sqrt(x**2 + y**2 + z**2) if abs(theta1) < 0.1 and abs( theta2 ) < 0.1: #The robot is not facing the exact destination, but it's close enough. so let's move. motor_command.linear.x = newSpeed / 2 motor_command.angular.z = theta1 motor_command.angular.y = theta2 motor_command_publisher.publish(motor_command)
def ep_error(self, ep_actual, ep_desired): pos_act, rot_act = ep_actual pos_des, rot_des = ep_desired err = np.mat(np.zeros((6, 1))) err[:3, 0] = pos_act - pos_des err[3:6, 0] = np.mat(tf_trans.euler_from_matrix(rot_des.T * rot_act)).T return err
def inverse_biased(kin, pose, q_init, q_bias, q_bias_weights, rot_weight=1.0, bias_vel=0.01, num_iter=100): # This code is potentially volatile q_out = q_init.copy() pos = pose[0:3,-1] rot = pose[0:3,0:3] for i in range(num_iter): # pos_fk, rot_fk = PoseConv.to_pos_rot(self.forward(q_out)) g = np.array(kin.forward(q_out)) pos_fk = g[0:3,-1] rot_fk = g[0:3,0:3] delta_twist = np.array(np.zeros(6)) pos_delta = pos - pos_fk delta_twist[:3] = pos_delta rot_delta = np.eye(4) rot_delta[:3,:3] = rot * rot_fk.T rot_delta_angles = np.array(trans.euler_from_matrix(rot_delta)) delta_twist[3:6] = rot_delta_angles J = np.array(kin.jacobian(q_out)) J[3:6,:] *= np.sqrt(rot_weight) delta_twist[3:6] *= np.sqrt(rot_weight) J_tinv = matmult(np.linalg.inv(matmult(J.T,J) + np.diag(q_bias_weights)), 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 + matmult(J_tinv, (delta_twist - matmult(J, q_bias_diff_normed))) q_out += delta_q q_out = np.array(kin.clip_joints_safe(q_out)) return q_out
def callback(self, rgb_msg, depth_msg): (translation, orientation) = self.listener.lookupTransform("/odom", "/base_footprint", rospy.Time(0)) robot_x = translation[0] r_xorient, r_yorient, r_zorient = transformations.euler_from_matrix( transformations.quaternion_matrix(orientation)) robot_theta = r_zorient # only need the z axis robot_y = translation[1] print("My position in start of callback is: ", robot_x, robot_y, robot_theta) # The callback processing the pairs of numbers that arrived at approximately the same time rawImage = self.bridge.imgmsg_to_cv2(rgb_msg, "bgr8") depthImage = self.bridge.imgmsg_to_cv2(depth_msg, "32FC1") labelArray, rectangleInfoArray, pointListArray = self.processRgb( rgb_msg, rawImage) for i in range(len(rectangleInfoArray)): object_coords = self.processDepth(depth_msg, rectangleInfoArray[i], robot_x, robot_y, robot_theta, pointListArray[i], depthImage) if object_coords[0] == -1: return else: #print("My position is: ", robot_x, robot_y, robot_theta) print("I detected a ", labelArray[i], " and its position is ", object_coords, ".") self.saveCoordinates(labelArray[i], object_coords) #print(self.objectCoordinates) return
def predict(self, Delta): # Implement Kalman prediction here theta = self.X[2, 0] pose_mat = np.mat([ [cos(theta), -sin(theta), 0, self.X[0, 0]], [sin(theta), cos(theta), 0, self.X[1, 0]], [0, 0, 1, 0], [0, 0, 0, 1], ]) pose_mat = Delta * pose_mat self.X[0:2, 0] = pose_mat[0:2, 3:4] euler = euler_from_matrix(pose_mat[0:3, 0:3], 'rxyz') self.X[2, 0] = euler[2] # Ignore the others Jx = np.mat( [[1, 0, -sin(theta) * Delta[0, 3] - cos(theta) * Delta[1, 3]], [0, 1, cos(theta) * Delta[0, 3] - sin(theta) * Delta[1, 3]], [0, 0, 1]]) Qs = np.mat(np.diag([self.position_uncertainty**2] * 3)) P = self.P[0:3, 0:3] self.P[0:3, 0:3] = Jx * P * Jx.T + Qs assert type(self.X) == np.matrixlib.defmatrix.matrix and type( self.P) == np.matrixlib.defmatrix.matrix self.P = np.mat(np.abs(self.P)) return self.X, self.P
def predict(self, Delta): # Implement Kalman prediction here theta = self.X[2, 0] pose_mat = mat([ [cos(theta), -sin(theta), 0, self.X[0, 0]], [sin(theta), cos(theta), 0, self.X[1, 0]], [0, 0, 1, 0], [0, 0, 0, 1], ]) pose_mat = Delta * pose_mat self.X[0:2, 0] = pose_mat[0:2, 3:4] euler = euler_from_matrix(pose_mat[0:3, 0:3], 'rxyz') self.X[2, 0] = euler[2] # Ignore the others Jx = mat([[1, 0, -sin(theta) * Delta[0, 3] - cos(theta) * Delta[1, 3]], [0, 1, cos(theta) * Delta[0, 3] - sin(theta) * Delta[1, 3]], [0, 0, 1]]) Qs = mat( diag([ self.position_uncertainty**2, self.position_uncertainty**2, self.angular_uncertainty**2 ])) P = self.P[0:3, 0:3] self.P[0:3, 0:3] = Jx * P * Jx.T + Qs return (self.X, self.P)
def go_angle(self, target_odom, tolerance=math.radians(5.), max_ang_vel=math.radians(20.), func=None): self.tl.waitForTransform('base_footprint', 'odom_combined', rospy.Time(), rospy.Duration(10)) rate = rospy.Rate(100) k = math.radians(80) #current_ang_odom = tr.euler_from_matrix(tfu.transform('base_footprint', 'odom_combined', self.tl)[0:3, 0:3], 'sxyz')[2] #target_odom = current_ang_odom + delta_ang while not rospy.is_shutdown(): robot_odom = tfu.transform('base_footprint', 'odom_combined', self.tl) current_ang_odom = tr.euler_from_matrix(robot_odom[0:3, 0:3], 'sxyz')[2] diff = ut.standard_rad(current_ang_odom - target_odom) p = k * diff #print diff if func != None: func(diff) if np.abs(diff) < tolerance: break tw = gm.Twist() vels = [p, np.sign(p) * max_ang_vel] tw.angular.z = vels[np.argmin(np.abs(vels))] #rospy.loginfo('diff %.3f vel %.3f' % (math.degrees(diff), math.degrees(tw.angular.z))) self.tw_pub.publish(tw) #rospy.loginfo('commanded %s' % str(tw)) rate.sleep() rospy.loginfo('finished %.3f' % math.degrees(diff)) return diff
def scan2point(laser_msg): global step num = len(laser_msg.ranges) index = np.linspace(laser_msg.angle_min,laser_msg.angle_max,num).tolist() # index = np.arange(laser_msg.angle_min,laser_msg.angle_max,laser_msg.angle_increment).tolist() if DEBUG_MODE == 'odom': _R_map, _T_map = robot_loc.odom() _R_fro = robot_loc.R_map_odom_ready _T_fro = robot_loc.T_map_odom_ready _R_map = np.dot(_R_fro, _R_map) _T_map += _T_fro elif DEBUG_MODE == 'map': _R_map, _T_map = robot_loc.map() _R_fro = robot_loc.R_map_odom_ready _T_fro = robot_loc.T_map_odom_ready _R_map = np.dot(_R_fro, _R_map) _T_map += _T_fro _R = np.eye(4) _R[:2, :2] = _R_map ax, ay, theta = euler_from_matrix(_R) map_range = [] for omiga in index: map_range.append(map_calc_range(static_map, _T_map[0, 0], _T_map[1, 0], theta + omiga)) laser_fix_range = judge_scan(laser_msg.ranges, map_range) point_data = np.empty((2, 1)) for i in range(0, len(index), step): if laser_msg.range_min <= laser_fix_range[i] <= 7: point_data = np.insert(point_data, -1, np.array([laser_fix_range[i]*math.cos(index[i]), laser_fix_range[i]*math.sin(index[i])]), axis=1) point_data = np.delete(point_data, -1, axis=1) return point_data
def __rodrigues2euler(self, rvec): R = cv2.Rodrigues(rvec)[0] T = np.zeros((4, 4)) T[3, 3] = 1.0 euler = np.array(euler_from_matrix(R, "sxyz")) euler[2] *= -1 return euler.reshape((3, 1))
def space_from_joints(joint_angles): T01, T12, T23, T34, T4e = direct_kinematics(joint_angles) T = tfs.concatenate_matrices(T01, T12, T23, T34, T4e) rz1, ry, rz2 = tfs.euler_from_matrix(T, 'szyz') trans = tfs.translation_from_matrix(T) S = np.append(trans, rz2) return S
def space_from_joints_small(joint_angles): T01, T12, T23, T34, T45, T5e, Tec, Tem = direct_kinematics(joint_angles) T = tfs.concatenate_matrices(T01, T12, T23, T34, T45, T5e, Tem) rx, ry, rz = tfs.euler_from_matrix(T, 'sxyz') trans = tfs.translation_from_matrix(T) S = np.concatenate((trans, [rz, ry]), axis=1) return S
def JS_to_PrPlRrl(q): robot = URDF.from_xml_file( "/home/josh/catkin_ws/src/baxter_common/baxter_description/urdf/baxter.urdf" ) kdl_kin_left = KDLKinematics(robot, "base", "left_gripper") kdl_kin_right = KDLKinematics(robot, "base", "right_gripper") T_left = kdl_kin_left.forward(q[0:7]) R_left = np.array(T_left[:3, :3]) T_right = kdl_kin_right.forward(q[7:14]) R_right = np.array(T_right[:3, :3]) x_left = T_left[0, 3] y_left = T_left[1, 3] z_left = T_left[2, 3] x_right = T_right[0, 3] y_right = T_right[1, 3] z_right = T_right[2, 3] R_rl = np.dot(np.transpose(R_right), R_left) roll, pitch, yaw = euler_from_matrix(R_rl, 'sxyz') P = np.array([[x_left], [y_left], [z_left], [x_right], [y_right], [z_right], [roll], [pitch], [yaw], [R_right[0, 0]], [R_left[0, 1]], [R_right[1, 0]], [R_left[1, 1]]]) return P
def get_tf(self, pose_tag_to_camera_link, quat_tag_to_camera_link, april_tag_id): """ Given the tf from april tag to camera link, calculate the tf from base link to map Input: pose_tag_to_camera_link: np(3,); the pose from tag to camera link quat_tag_to_camera_link: np(4,); the quaternion from tag to camera link april_tag_id: int; the id of the apriltag Output: P_base_link_to_map: np(3,); the P from base link to map """ T_pose_tag_to_camera_link = np.array( [[1., 0., 0., pose_tag_to_camera_link[0]], [0., 1., 0., pose_tag_to_camera_link[1]], [0., 0., 1., pose_tag_to_camera_link[2]], [0., 0., 0., 1.]]) T_quat_tag_to_camera_link = tfm.quaternion_matrix( quat_tag_to_camera_link) T_tag_to_camera_link = np.dot(T_pose_tag_to_camera_link, T_quat_tag_to_camera_link) T_camera_link_to_tag = np.linalg.inv(T_tag_to_camera_link) T_tag_to_map = self.apriltag_transform_dict[april_tag_id] T_base_link_to_map = np.dot(np.dot(T_tag_to_map, T_camera_link_to_tag), self.T_base_link_to_camera_link) theta = tfm.euler_from_matrix(T_base_link_to_map, axes='sxyz')[2] P_base_link_to_map = np.array( [T_base_link_to_map[0][3], T_base_link_to_map[1][3], theta]) return P_base_link_to_map
def test_euler_XYZ_from_rotation_matrix(self): """ r in 'rxyz' is 'r'otating frames indicating rotations are applied consecutively with respect to current frames' axes, "relative-rotation". Order of rotations are first X, second Y and third Z. Rotation matrix composition rule for relative rotations: Rx * Ry * Rz. s in 'sxyz' is 's'tationary frames indicating rotations are applied with respect to the coordinate frame of the INITIAL frame, "fixed-axis rotation". Rotation matrix composition rule for fixed-axis rotations: Rz * Ry * Rx. """ Rx_90 = tr.rotation_matrix( math.pi / 4, [1, 0, 0]) # first, rotate 90 degrees around x axis Ry_90 = tr.rotation_matrix( math.pi / 5, [0, 1, 0]) # second, 45 degrees around y axis of the current frame Rz_90 = tr.rotation_matrix( math.pi / 6, [0, 0, 1]) # third, 30 degrees around z axis of the current frame R1 = np.matmul(Rz_90, Ry_90) R = np.matmul(R1, Rx_90) euler_angles = tr.euler_from_matrix(R, 'sxyz') euler_angles_expected = [math.pi / 4, math.pi / 5, math.pi / 6] np.testing.assert_almost_equal(euler_angles, euler_angles_expected)
def clickedCB(data): global askedMarkers global listener global worldTransform global measuredMarkers global clickNum print 'clicked' try: (tipTrans, tipRot) = listener.lookupTransform('/mocha_world', '/tip', rospy.Time(0)) measuredMarkers[clickNum, :] = tipTrans clickNum = clickNum + 1 if clickNum == 8: clickNum = 0 print 'finished clicking' print 'AskedMarkers:' print askedMarkers print 'measured markers:' print measuredMarkers (worldRotupdate, worldTransUpdate) = rigid_transform_3D(measuredMarkers, askedMarkers) worldRot4 = np.identity(4) for i in range(3): worldRot4[(0, 1, 2), i] = worldRotupdate[:, i] worldTransformUpdate = TR.compose_matrix( translate=worldTransUpdate, angles=TR.euler_from_matrix(worldRot4)) worldTransformationUpdateInv = TR.inverse_matrix( worldTransformUpdate) worldTransform = np.dot(worldTransformationUpdateInv, worldTransform) except (): nothing = 0
def predict(self, Delta): # Implement Kalman prediction here # oldP = self.P.copy() theta = self.X[2, 0] pose_mat = np.mat([ [cos(theta), -sin(theta), 0, self.X[0, 0]], [sin(theta), cos(theta), 0, self.X[1, 0]], [0, 0, 1, 0], [0, 0, 0, 1], ]) pose_mat = Delta * pose_mat self.X[0:2, 0] = pose_mat[0:2, 3:4] euler = euler_from_matrix(pose_mat[0:3, 0:3], 'rxyz') self.X[2, 0] = euler[2] # Ignore the others Jx = np.mat( [[1, 0, -sin(theta) * Delta[0, 3] - cos(theta) * Delta[1, 3]], [0, 1, cos(theta) * Delta[0, 3] - sin(theta) * Delta[1, 3]], [0, 0, 1]]) Qs = np.mat(np.diag([self.position_uncertainty**2] * 3)) P = self.P[0:3, 0:3] self.P[0:3, 0:3] = Jx * P * Jx.T + Qs # if np.min(self.P.diagonal()) < 0.0: # print "bad P on predict arg at:", np.argmin(self.P.diagonal()) # print "Jx matrix:\n", Jx # print "old value of P:\n", oldP.diagonal() # print "attention les yeux:\n", self.P.diagonal() # print "================= Patching P ===============" # n, _ = self.P.shape # for i in range(n): # if self.P[i, i] <= 0.0: # self.P[i, i] = 0.001 return self.X, self.P
def rotationMatrixToEulerAngles(R): """Calculates euler angles given rotation matrix in 'xyz' sequence :param R: 3x3 rotation matrix :return: list of three euler angles equivalent to the given rotation matrix """ return list(euler_from_matrix(R, axes="sxyz"))
def execute(self, userdata): userdata.marker_pose.header.stamp = rospy.Time.now() pose = self.tf.transformPose('/base_link', userdata.marker_pose) p = [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z] q = [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w] rm = tfs.quaternion_matrix(q) # assemble a new coordinate frame that has z-axis aligned with # the vertical direction and x-axis facing the z-axis of the # original frame z = rm[:3, 2] z[2] = 0 axis_x = tfs.unit_vector(z) axis_z = tfs.unit_vector([0, 0, 1]) axis_y = np.cross(axis_x, axis_z) rm = np.vstack((axis_x, axis_y, axis_z)).transpose() # shift the pose along the x-axis p += np.dot(rm, [self.DISTANCE, 0, 0])[:3] userdata.goal_pose = pose userdata.goal_pose.pose.position.x = p[0] userdata.goal_pose.pose.position.y = p[1] userdata.goal_pose.pose.position.z = p[2] yaw = tfs.euler_from_matrix(rm)[2] q = tfs.quaternion_from_euler(0, 0, yaw - math.pi) userdata.goal_pose.pose.orientation.x = q[0] userdata.goal_pose.pose.orientation.y = q[1] userdata.goal_pose.pose.orientation.z = q[2] userdata.goal_pose.pose.orientation.w = q[3] return 'succeeded'
def Aruco_marker_detector(self): """ Use the build in python library to detect the aruco marker and its coordinates """ img = self.frame.copy() # Detect the markers in the image markerCorners, markerIds, rejectedCandidates = cv.aruco.detectMarkers(img, self.dictionary, parameters=self.parameters) # Get the transformation matrix to the marker if markerCorners: markerSize = 2.0 axisLength = 3.0 rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(markerCorners, markerSize, self.K, self.distCoeffs) # Draw the axis on the marker frame_out = cv.aruco.drawAxis( img, self.K, self.distCoeffs, rvecs, tvecs, axisLength) rotMat = tr.euler_matrix(np.pi / 2.0, 0, 0) rotMat = rotMat[0:3, 0:3] tvecs = np.matmul(rotMat, tvecs[0][0].T) rotMat, _ = cv.Rodrigues(rvecs) eul = tr.euler_from_matrix(rotMat) yaw_angle = self.pos[3] - eul[2] # Publish the position of the marker marker_pos = PT() marker_pos.position.x = tvecs[0] marker_pos.position.y = - tvecs[2] marker_pos.position.z = tvecs[1] marker_pos.yaw = eul[2] * np.pi / 180.0 self.aruco_marker_pos_pub.publish(marker_pos) # Publish the image with the detected marker self.aruco_marker_img_pub.publish(self.bridge.cv2_to_imgmsg(frame_out, "bgr8"))
def getRotationMotion(R): Result={} angles=copy.deepcopy(euler_from_matrix(R,'szxy')) Result["Roll"]=57.2958*angles[0] Result["Pitch"]=57.2958*angles[1] Result["Yaw"]=57.2958*angles[2] return Result
def generate_pose(T, camera_optical_frame): print('publish TF for \n', T) rate = rospy.Rate(30) # Hz listener = tf.TransformListener() try: t, r = listener.lookupTransform('/base_link', camera_optical_frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.error( "failed to listen transform from '/base_link' to '/camera_optical_frame'" ) continue cam2base = numpy.matrix(tft.quaternion_matrix(r)) cam2base[0, 3] = t[0] cam2base[1, 3] = t[1] cam2base[2, 3] = t[2] T = numpy.array(numpy.dot(cam2base, T)) p = Pose() p.position.x = T[0, 3] p.position.y = T[1, 3] p.position.z = T[2, 3] R = T[:3, :3] roll, pitch, yaw = tft.euler_from_matrix(T) q = tft.quaternion_from_euler(roll, pitch, yaw) p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] #tf_brocast(p, camera_optical_frame) tf_brocast(p, "base_link")
def ep_error(self, ep_actual, ep_desired): pos_act, rot_act = ep_actual pos_des, rot_des = ep_desired err = np.mat(np.zeros((6, 1))) err[:3,0] = pos_act - pos_des err[3:6,0] = np.mat(tf_trans.euler_from_matrix(rot_des.T * rot_act)).T return err
def _construct_xyzrpy_from_matrix(self, matrix): translation = transformations.translation_from_matrix(matrix) euler = transformations.euler_from_matrix(matrix) pose = np.concatenate([translation, euler]) return pose
def inv_kin( self, q_current, T_desired, ): T_current, T_joint = self.fwd_kin(q_current) num_joints = len(self.a) - 1 c_T_d = tf_tran.concatenate_matrices( tf_tran.inverse_matrix(T_current), T_desired ) # transformation of desired frame with respect to current frame c_t_d = c_T_d[0:3, 3] # extracting the translation part # c_R_d = c_T_d[0:3, 0:3] # extracting the rotation part ROT = np.array(tf_tran.euler_from_matrix(c_T_d)) delta_x = c_t_d P = 1.0 dx = P * delta_x # ( velocity change wrt ee_current frame) v_ee = np.zeros([6, 1]) v_ee[0:3] = dx.reshape([3, 1]) v_ee[3:6] = ROT.reshape([3, 1]) # Jac = self.jacobian(T_joint, T_current) Jac = self.geometric_jacobian(T_joint, T_current) qq = np.ones([num_joints, 1]) * 1 J_pinv = np.linalg.pinv(Jac) qn_dot = np.dot((np.identity(num_joints) - np.dot(J_pinv, Jac)), qq) # null space jacobian final_theta = np.dot( J_pinv, v_ee) # + qn_dot # final_theta are the joint velocities final_theta = np.insert(final_theta, num_joints, 0) # Adding 0 at the end for the fixed joint return final_theta
def __get_cartesian_from_matrix(self, h_matrix): # Returns a list of [roll, pitch, yaw, X, Y, Z] euler_from_matrix = list(TF.euler_from_matrix(h_matrix)) translation_from_matrix = list(TF.translation_from_matrix(h_matrix)) return euler_from_matrix + translation_from_matrix
def place(self, x, y, z, alpha, beta, gamma, approach): # find pre-place pose T_grasp = euler_matrix(radians(alpha), radians(beta), radians(gamma), axes="sxyz") T_grasp[:3, 3] = np.array([x, y, z]) T_trans = np.identity(4) T_trans[2, 3] = -approach T_pre_grasp = np.dot(T_grasp, T_trans) alpha_pre, beta_pre, gamma_pre = euler_from_matrix(T_pre_grasp) alpha_pre, beta_pre, gamma_pre = (degrees(alpha_pre), degrees(beta_pre), degrees(gamma_pre)) x_pre, y_pre, z_pre = T_pre_grasp[:3, 3] # move to pre-place if (self.move(x_pre, y_pre, z_pre, alpha_pre, beta_pre, gamma_pre) and # move to place self.move(x, y, z, alpha, beta, gamma) and # open gripper self.open_gripper() and # move back to pre-grasp self.move(x_pre, y_pre, z_pre, alpha_pre, beta_pre, gamma_pre)): rospy.loginfo("Place finished") return True else: rospy.logerr("Place failed") self.clear_error() return False
def motion_model(self, f0, f1, stamp, use_kalman=False): if not use_kalman: # simple `repetition` model txn0, rxn0 = f0['pose'][L_POS], f0['pose'][A_POS] txn1, rxn1 = f1['pose'][L_POS], f1['pose'][A_POS] R0 = tx.euler_matrix(*rxn0) R1 = tx.euler_matrix(*rxn1) T0 = tx.compose_matrix(angles=rxn0, translate=txn0) T1 = tx.compose_matrix(angles=rxn1, translate=txn1) Tv = np.dot(T1, vm.inv(T0)) # Tv * T0 = T1 T2 = np.dot(Tv, T1) txn = tx.translation_from_matrix(T2) rxn = tx.euler_from_matrix(T2) x = f1['pose'].copy() P = f1['cov'].copy() x[0:3] = txn x[9:12] = rxn return x, P else: # dt MUST NOT BE None self.kf_.x = f0['pose'] self.kf_.P = f0['cov'] dt = (f1['stamp'] - f0['stamp']) self.kf_.predict(dt) txn, rxn = f1['pose'][L_POS], f1['pose'][A_POS] z = np.concatenate([txn, rxn]) self.kf_.update(z) dt = (stamp - f1['stamp']) self.kf_.predict(dt) return self.kf_.x.copy(), self.kf_.P.copy()
def JS_to_PrPlRrl(q): T_left = kdl_kin_left.forward(q[0:7]) R_left = np.array(T_left[:3, :3]) T_right = kdl_kin_right.forward(q[7:14]) R_right = np.array(T_right[:3, :3]) x_left = T_left[0, 3] y_left = T_left[1, 3] z_left = T_left[2, 3] x_right = T_right[0, 3] y_right = T_right[1, 3] z_right = T_right[2, 3] T_rl = np.dot(InverseTransformationMatrix(T_right), T_left) sep_dist_x = T_rl[0, 3] sep_dist_y = T_rl[1, 3] sep_dist_z = T_rl[2, 3] R_rl = np.dot(np.transpose(R_right), R_left) roll, pitch, yaw = euler_from_matrix(R_rl, 'sxyz') P = np.array([[x_left], [y_left], [z_left], [x_right], [y_right], [z_right], [roll], [pitch], [yaw], [sep_dist_x], [sep_dist_y], [sep_dist_z]]) return P
def to_pose2d(pos): # pos = N?x3x4 transformation matrix # 2d rotation requires (-y) component # 2d translation requires (z, -x) component # == test == # -- opt 1 : einsum -- T_pre = tx.euler_matrix(-np.pi/2,0,-np.pi/2) #4x4 T_0i = tx.inverse_matrix(np.concatenate((pos[0], [[0,0,0,1]]), axis=0)) #4x4 T_cam = np.stack([np.concatenate((p, [[0,0,0,1]]), axis=0) for p in pos], axis=0) pos_n = np.einsum('ij,jk,nkl,lm->nim', T_pre, T_0i, T_cam, T_pre.T) # normalized position res = [] for p in pos_n: t = tx.translation_from_matrix(p) q = tx.euler_from_matrix(p) res.append([ t[0], t[1], q[-1] ]) res = np.stack(res, axis=0) return res # -------------------- # -- opt 2 : no einsum -- #T_pre = tx.euler_matrix(-np.pi/2,0,-np.pi/2) #4x4 #T_0i = tx.inverse_matrix(np.concatenate((pos[0], [[0,0,0,1]]), axis=0)) #4x4 #T_cam = np.stack([np.concatenate((p, [[0,0,0,1]]), axis=0) for p in pos], axis=0) #res = [] #for p in pos: # T_cam = np.concatenate((p, [[0,0,0,1]]), axis=0) # T_base = T_pre.dot(T_0i).dot(T_cam).dot(T_pre.T) # t = tx.translation_from_matrix(T_base) # q = tx.euler_from_matrix(T_base) # res.append([t[0], t[1], q[-1]]) #res = np.stack(res, axis=0) return res
def computeTransformation(world, slam, camera): # Here we do not care about the slamMworld transformation # timing as it is constant. tWorld = listener.getLatestCommonTime(world, camera) tMap = listener.getLatestCommonTime(slam, camera) t = min(tWorld, tMap) # Current pose given by the SLAM. (slamMcam_T, slamMcam_Q) = listener.lookupTransform(camera, slam, t) slamMcam = np.matrix(transformer.fromTranslationRotation(slamMcam_T, slamMcam_Q)) # Estimation of the camera position given by control. #FIXME: order is weird but it works. (worldMcam_T, worldMcam_Q) = listener.lookupTransform(world, camera, t) worldMcam = np.matrix(transformer.fromTranslationRotation(worldMcam_T, worldMcam_Q)) (slamMworld_T, slamMworld_Q) = listener.lookupTransform(slam, world, t) slamMworld = np.matrix(transformer.fromTranslationRotation(slamMworld_T, slamMworld_Q)) slamMcamEstimated = slamMworld * worldMcam # Inverse frames. camMslam = np.linalg.inv(slamMcam) camMslamEstimated = np.linalg.inv(slamMcamEstimated) # Split. camMslam_T = translation_from_matrix(camMslam) camMslam_Q = quaternion_from_matrix(camMslam) camMslam_E = euler_from_matrix(camMslam) camMslamEstimated_T = translation_from_matrix(camMslamEstimated) camMslamEstimated_Q = quaternion_from_matrix(camMslamEstimated) camMslamEstimated_E = euler_from_matrix(camMslamEstimated) # Compute correction. camMslamCorrected_T = [camMslam_T[0], camMslamEstimated_T[1], camMslam_T[2]] camMslamCorrected_E = [camMslamEstimated_E[0], camMslam_E[1], camMslamEstimated_E[2]] camMslamCorrected_Q = quaternion_from_euler(*camMslamCorrected_E) return (camMslamCorrected_T, camMslamCorrected_Q, t)
def draw_map(ax, frame, db, cfg): # global - top-down view # extract data map_frame = db.keyframe[0] xyz = db.landmark['pos'] col = db.landmark['col'] #idx = np.random.choice(len(xyz), size=2048, replace=False) #xyz = xyz[idx] #col = col[idx] # draw (3d) ax3 = sub_axis(ax, [0.0, 0.0, 1.0, 0.5], projection='3d') ax3.scatter(xyz[:,0], xyz[:,1], xyz[:,2], s = 0.1, c = (col[...,::-1] / 255.0), ) for fr in db.frame: xfm_pose = pose_to_xfm(fr['pose']) txn = tx.translation_from_matrix(xfm_pose) rxn = tx.euler_from_matrix(xfm_pose) draw_pose(ax3, txn, rxn, alpha=0.02) draw_pose(ax3, frame['pose'][0:3], frame['pose'][9:12]) set_axes_equal(ax3) # draw (2d) T_R = vm.tx.euler_matrix(-np.pi/2, 0, -np.pi/2) ax2 = sub_axis(ax, [0.0, 0.5, 1.0, 0.5]) xyz = vm.tx3(T_R, xyz) ax2.scatter(xyz[:,0], xyz[:,1], s = 0.1, c = (col[...,::-1] / 255.0) ) for fr in db.frame: xfm_pose = pose_to_xfm(fr['pose']) r_xfm_pose = T_R.dot(xfm_pose) txn = tx.translation_from_matrix(r_xfm_pose) rxn = tx.euler_from_matrix(r_xfm_pose) draw_pose(ax2, txn, rxn, alpha=0.02) fr = frame xfm_pose = pose_to_xfm(fr['pose']) r_xfm_pose = T_R.dot(xfm_pose) txn = tx.translation_from_matrix(r_xfm_pose) rxn = tx.euler_from_matrix(r_xfm_pose) draw_pose(ax2, txn, rxn, alpha=1.0) ax2.set_aspect('equal')
def toROSPose2DModel(self): ros = Pose2DModel() ros.id = self.id ros.type = str(self.type) matrix = toMatrix(self.pose.pose) ros.pose.x = matrix[0][3] ros.pose.y = matrix[1][3] ros.pose.theta = euler_from_matrix(matrix)[2] return ros
def go_angle(self, target_base, tolerance=math.radians(5.), max_ang_vel=math.radians(20.), func=None): self.tl.waitForTransform('odom_combined', 'base_footprint', rospy.Time(), rospy.Duration(10)) rate = rospy.Rate(100) k = math.radians(80) od_T_bf = tfu.transform('odom_combined', 'base_footprint', self.tl) target_odom_mat = od_T_bf * tfu.tf_as_matrix( ([0, 0, 0.], tr.quaternion_from_euler(0, 0, target_base)) ) target_odom = tr.euler_from_quaternion(tfu.matrix_as_tf(target_odom_mat)[1])[2] #target_odom = (od_T_bf * np.row_stack([target_base, np.matrix([0,1.]).T]))[0:2,0] #current_ang_odom = tr.euler_from_matrix(tfu.transform('base_footprint', 'odom_combined', self.tl)[0:3, 0:3], 'sxyz')[2] #target_odom = current_ang_odom + delta_ang robot_odom = tfu.transform('odom_combined', 'base_footprint', self.tl) current_ang_odom = tr.euler_from_matrix(robot_odom[0:3, 0:3], 'sxyz')[2] diff = ut.standard_rad(current_ang_odom - target_odom) rospy.loginfo('go_angle: target %.2f' % np.degrees(target_odom)) rospy.loginfo('go_angle: current %.2f' % np.degrees(current_ang_odom)) rospy.loginfo('go_angle: diff %.2f' % np.degrees(diff)) while not rospy.is_shutdown(): robot_odom = tfu.transform('odom_combined', 'base_footprint', self.tl) current_ang_odom = tr.euler_from_matrix(robot_odom[0:3, 0:3], 'sxyz')[2] diff = ut.standard_rad(target_odom - current_ang_odom) rospy.loginfo('go_angle: current %.2f diff %.2f' % (np.degrees(current_ang_odom), np.degrees(diff))) p = k * diff if func != None: func(diff) if np.abs(diff) < tolerance: break if self.go_ang_server.is_preempt_requested(): self.go_ang_server.set_preempted() break tw = gm.Twist() vels = [p, np.sign(p) * max_ang_vel] tw.angular.z = vels[np.argmin(np.abs(vels))] #rospy.loginfo('diff %.3f vel %.3f' % (math.degrees(diff), math.degrees(tw.angular.z))) self.tw_pub.publish(tw) #rospy.loginfo('commanded %s' % str(tw)) rate.sleep() rospy.loginfo('go_ang: finished %.3f' % math.degrees(diff)) return diff
def set_target_pose_relative(self, name, delta_xyz, delta_rpy, tm): if name.lower() == 'rarm': joint = 'RARM_JOINT5' elif name.lower() == 'larm': joint = 'LARM_JOINT5' else: raise rospy.ServiceException() matrix = self.get_current_pose(joint).pose.data xyz = numpy.array([matrix[3], matrix[7], matrix[11]]) rpy = numpy.array(euler_from_matrix([matrix[0:3], matrix[4:7], matrix[8:11]], 'sxyz')) xyz += [delta_xyz[0], delta_xyz[1], delta_xyz[2]] rpy += [delta_rpy[0], delta_rpy[1], delta_rpy[2]] return self.set_target_pose(name, list(xyz), list(rpy), tm)
def go_ang(self, ang, speed): dt = math.radians(ang) if dt > 0: sign = -1 elif dt < 0: sign = 1 else: sign = 0 self.tl.waitForTransform("base_footprint", "odom_combined", rospy.Time(), rospy.Duration(10)) p0_base = tfu.transform("base_footprint", "odom_combined", self.tl) # \ start_ang = tr.euler_from_matrix(p0_base[0:3, 0:3], "sxyz")[2] r = rospy.Rate(100) dist_so_far = 0.0 last_ang = start_ang while not rospy.is_shutdown(): pcurrent_base = tfu.transform("base_footprint", "odom_combined", self.tl) # \ current_ang = tr.euler_from_matrix(pcurrent_base[0:3, 0:3], "sxyz")[2] dist_so_far = dist_so_far + (ut.standard_rad(current_ang - last_ang)) if dt > 0 and dist_so_far > dt: rospy.loginfo("stopped! %f %f" % (dist_so_far, dt)) break elif dt < 0 and dist_so_far < dt: rospy.loginfo("stopped! %f %f" % (dist_so_far, dt)) break elif dt == 0: rospy.loginfo("stopped! %f %f" % (dist_so_far, dt)) break tw = gm.Twist() tw.angular.z = math.radians(speed * sign) self.tw_pub.publish(tw) r.sleep() last_ang = current_ang
def tf_cb(self, msg): for t in msg.transforms: if t.child_frame_id == self.target: time = self.tf.getLatestCommonTime(self.source, self.target) p, q = self.tf.lookupTransform(self.source, self.target, time) rm = tfs.quaternion_matrix(q) # assemble a new coordinate frame that has z-axis aligned with # the vertical direction and x-axis facing the z-axis of the # original frame z = rm[:3, 2] z[2] = 0 axis_x = tfs.unit_vector(z) axis_z = tfs.unit_vector([0, 0, 1]) axis_y = np.cross(axis_x, axis_z) rm = np.vstack((axis_x, axis_y, axis_z)).transpose() # shift the pose along the x-axis self.position = p + np.dot(rm, self.d)[:3] self.buffer[self.buffer_ptr] = tfs.euler_from_matrix(rm)[2] self.buffer_ptr = (self.buffer_ptr + 1) % self.buffer_size self.publish_filtered_tf(t.header)
def predict(self, Delta): # Implement Kalman prediction here theta = self.X[2,0] pose_mat = mat([[cos(theta), -sin(theta), 0, self.X[0,0]], [sin(theta), cos(theta), 0, self.X[1,0]], [ 0, 0, 1, 0], [ 0, 0, 0, 1], ]); pose_mat = Delta * pose_mat; self.X[0:2,0] = pose_mat[0:2,3:4] euler = euler_from_matrix(pose_mat[0:3,0:3], 'rxyz') self.X[2,0] = euler[2]; # Ignore the others Jx = mat([[1, 0, -sin(theta)*Delta[0,3]-cos(theta)*Delta[1,3]], [0, 1, cos(theta)*Delta[0,3]-sin(theta)*Delta[1,3]], [0, 0, 1 ]]) Qs = mat(diag([self.position_uncertainty**2,self.position_uncertainty**2,self.angular_uncertainty**2])) P = self.P[0:3,0:3] self.P[0:3,0:3] = Jx * P * Jx.T + Qs return (self.X,self.P)
def dls_ik(kin, pose, q0, lam=0.25, num=100): # get position and orientation: Rd = pose[0:3,0:3] Pd = pose[0:3,-1] # setup iterations: q = q0.copy() # run loop trying to get to target: for i in range(num): J = kin.jacobian(q) g = np.array(kin.forward(q)) R = g[0:3,0:3] P = g[0:3,-1] Rdelt = matmult(Rd, R.T) rdelt_angles = np.array(trans.euler_from_matrix(Rdelt)) e = np.hstack((Pd-P, rdelt_angles)) dq = np.array(matmult(J.T,np.linalg.inv(matmult(J,J.T) + lam*np.eye(J.shape[0])))) q += matmult(dq,e) ############################################################## # should add a break condition and corresponding tolerance!! # ############################################################## return q
def IK_biased(self, pos, rot, q_init, q_bias, q_bias_weights, rot_weight=1., bias_vel=0.01, num_iter=100): q_out = np.mat(self.IK_search(pos, rot)).T for i in range(num_iter): pos_fk, rot_fk = self.FK_vanilla(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(tf_trans.euler_from_matrix(rot_delta)).T delta_twist[3:6,0] = rot_delta_angles J = self.jacobian_vanilla(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(np.clip(q_out.T.A[0], self.joint_info["safe_mins"], self.joint_info["safe_maxs"])).T return q_out
def processAprilTags(self, msg): # Only process if the wheels_cmd hasn't changed since last time and the last time < cur time if self.wheelsCmdAlmostEqual(self.wheels_cmd, self.wheels_cmd_prev) and msg.header.stamp > self.timestamp_prev: deltas = [] for tag in msg.detections: if tag.id in self.april_tags_prev: Ta_r1 = self.geometryTransformToMatrix(self.april_tags_prev[tag.id].transform) Ta_r2 = self.geometryTransformToMatrix(tag.transform) Tr2_r1 = np.dot(Ta_r1, tr.inverse_matrix(Ta_r2)) # Get the angle, dx, and dy theta = tr.euler_from_matrix(Tr2_r1)[2] dx,dy = Tr2_r1[0:2,3] deltas.append([theta, dx, dy]) if deltas: # We found some matches, average them, and print deltas = np.mean(np.array(deltas),0).tolist() cmd = [self.wheels_cmd_prev.vel_left, self.wheels_cmd_prev.vel_right, (msg.header.stamp - self.timestamp_prev).to_sec()] sample = cmd+deltas rospy.loginfo(sample) f = open(self.filename, 'a+') np.savetxt(f, sample, fmt='%-7.8f', newline=" ") f.write('\n') f.close() else: err = "backwards time." if msg.header.stamp < self.timestamp_prev else "cmd changed" rospy.logwarn("Invalid interval %s", err) # Save the tags and wheels_cmd for next time for tag in msg.detections: self.april_tags_prev[tag.id] = tag self.timestamp_prev = msg.header.stamp self.wheels_cmd_prev = self.wheels_cmd
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 homo_mat_to_2d(mat): rot = tf_trans.euler_from_matrix(mat)[2] return mat[0,3], mat[1,3], rot
def main(): # Header of the sensor file NSMAP = {"xacro" : "http://www.ros.org/wiki/xacro"} ROOT = ltr.Element("robot",nsmap=NSMAP) XACRO= ltr.SubElement(ROOT,"xacroCOLONmacro",name="ur10_arm_gazebo",params="prefix") ROOT.insert(0,ltr.Comment("Proximity sensors")) string_joint_link="" string_collisions="" modify=True #if false the original files are used by moveit if true the file with sensor is used SensorCount=0 width=0.10; min_range=0.06; max_range=0.8; Rate=10; theta_incline=0.05 s=Sensor_attributes(width,min_range,max_range,Rate) # # Link 2 frame_name="forearm_link" # Note 1 In the absence of an array pick max position # Note 2 R is defined by relative rotations # Ultrasound ring # 0 theta_incline=15.* np.pi/180. R=np.dot(np.dot(transformations.rotation_matrix(np.pi/2,np.array([1,0,0])),transformations.rotation_matrix(np.pi/2,np.array([0,0,1]))),transformations.rotation_matrix(-np.pi,np.array([0,1,0]))) Rincline=np.dot(R,transformations.rotation_matrix(-theta_incline,np.array([0,0,1]))) Rx,Ry,Rz=transformations.euler_from_matrix(Rincline,'sxyz') Position_min=np.array([-0.06,-0.06,0.53,Rx,Ry,Rz]); Position_max=np.array([0.078,0.00,0.48,Rx,Ry,Rz]) face_array=sensor_array(frame_name,Position_min,Position_max,[s]) string_joint_link,string_collisions,XACRO,SensorCount=create_sensors_for_face(XACRO,SensorCount,face_array,string_joint_link,string_collisions) print "Generated Sensor ",SensorCount # # 1 to mod R=np.dot(np.dot(transformations.rotation_matrix(np.pi/2,np.array([1,0,0])),transformations.rotation_matrix(np.pi/2,np.array([0,0,1]))),transformations.rotation_matrix(-np.pi,np.array([0,1,0]))) R=np.dot(R,transformations.rotation_matrix(-np.pi/2,np.array([1,0,0]))) # rotating for the ring Rincline=np.dot(R,transformations.rotation_matrix(-theta_incline,np.array([0,0,1]))) Rx,Ry,Rz=transformations.euler_from_matrix(Rincline,'sxyz') Position_min=np.array([-0.06,-0.06,0.53,Rx,Ry,Rz]); Position_max=np.array([0.0,0.075,0.48,Rx,Ry,Rz]) face_array=sensor_array(frame_name,Position_min,Position_max,[s]) string_joint_link,string_collisions,XACRO,SensorCount=create_sensors_for_face(XACRO,SensorCount,face_array,string_joint_link,string_collisions) print "Generated Sensor ",SensorCount # # #2 # R=np.dot(np.dot(transformations.rotation_matrix(np.pi/2,np.array([1,0,0])),transformations.rotation_matrix(np.pi/2,np.array([0,0,1]))),transformations.rotation_matrix(-np.pi,np.array([0,1,0]))) R=np.dot(R,transformations.rotation_matrix(-np.pi,np.array([1,0,0]))) # rotating for the ring Rincline=np.dot(R,transformations.rotation_matrix(-theta_incline,np.array([0,0,1]))) Rx,Ry,Rz=transformations.euler_from_matrix(Rincline,'sxyz') Position_min=np.array([-0.06,-0.06,0.53,Rx,Ry,Rz]); Position_max=np.array([-0.075,-0.0,0.48,Rx,Ry,Rz]) face_array=sensor_array(frame_name,Position_min,Position_max,[s]) string_joint_link,string_collisions,XACRO,SensorCount=create_sensors_for_face(XACRO,SensorCount,face_array,string_joint_link,string_collisions) print "Generated Sensor ",SensorCount # # #3 R=np.dot(np.dot(transformations.rotation_matrix(np.pi/2,np.array([1,0,0])),transformations.rotation_matrix(np.pi/2,np.array([0,0,1]))),transformations.rotation_matrix(-np.pi,np.array([0,1,0]))) R=np.dot(R,transformations.rotation_matrix(np.pi/2,np.array([1,0,0]))) # rotating for the ring Rincline=np.dot(R,transformations.rotation_matrix(-theta_incline,np.array([0,0,1]))) Rx,Ry,Rz=transformations.euler_from_matrix(Rincline,'sxyz') Position_min=np.array([-0.06,-0.06,0.53,Rx,Ry,Rz]); Position_max=np.array([0.0,-0.075,0.48,Rx,Ry,Rz]) face_array=sensor_array(frame_name,Position_min,Position_max,[s]) string_joint_link,string_collisions,XACRO,SensorCount=create_sensors_for_face(XACRO,SensorCount,face_array,string_joint_link,string_collisions) print "Generated Sensor ",SensorCount # # ------------------------------------------------------------------------- # # Convert to text, Replace Colons and write result to .xacro file # # ------------------------------------------------------------------------- Original_xacro_file = open("/home/philip/catkin_ws_v5/src/ur_package/config/SensorGeneration/ur10.urdf.xacro", "r") # Bare version of .xacro Original_srdf_file = open("/home/philip/catkin_ws_v5/src/ur_package/config/SensorGeneration/ur10.srdf", "r") # Bare version of .sdf Gazebo_sensor_file = open("/opt/ros/indigo/share/ur_description/urdf/ur10.gazebo.xacro", 'w') UR10_xacro_file = open("/opt/ros/indigo/share/ur_description/urdf/ur10.urdf.xacro", "w") # replace used version with sensor version UR10_srdf_file = open("/opt/ros/indigo/share/ur10_moveit_config/config/ur10.srdf", "w") contents = Original_srdf_file.readlines() # read bare version if(modify): contents.insert(-1,string_collisions) # insert the supplmentary joint information at end of file contents = "".join(contents) # joint contents to create buffer UR10_srdf_file.write(contents) # write buffer to used version contents = Original_xacro_file.readlines() # read bare version if(modify): contents.insert(-6,string_joint_link) # insert the supplmentary joint information at end of file contents = "".join(contents) # joint contents to create buffer UR10_xacro_file.write(contents) # write buffer to used version string=ltr.tostring(ROOT, pretty_print=True,xml_declaration=True) Gazebo_sensor_file.write(string.replace("COLON",":")) # A terrible hack I should be ashamed of required cause I couldn't be bothered learning more about namespaces Gazebo_sensor_file.close() Original_srdf_file.close() Original_xacro_file.close() UR10_xacro_file.close() UR10_srdf_file.close()
def getRotationAroundZ(self): return euler_from_matrix(self._matrix, 'szyx')[0]
def stat_poses(csv_file): print('%s %s %s' % ('>' * 20, csv_file, '<' * 20)) df = pandas.DataFrame.from_csv(csv_file, index_col=None) # Compute average of transformation matrix # ======================================== n_fused = 0 matrix_fused = None # average matrix from base -> checkerboard pose for index, row in df.iterrows(): # base -> pointIn (sensed calibboard pose) pos = row['position_x'], row['position_y'], row['position_z'] matrix_pos = tff.translation_matrix(pos) rot = row['quaternion_x'], row['quaternion_y'], row['quaternion_z'], row['quaternion_w'] matrix_rot = tff.quaternion_matrix(rot) matrix_pose = matrix_pos.dot(matrix_rot) if n_fused == 0: matrix_fused = matrix_pose n_fused += 1 continue # Fuse transformation matrix # -------------------------- # base -> pointFused (fused calibboard pose) # matrix_fused # pointFused -> pointIn = pointFused -> base -> pointIn matrix_rel = matrix_pose.dot(tff.inverse_matrix(matrix_fused)) # weight of the pointIn is 1 / (n_fused + 1) weight_of_new_point = 1. / (n_fused + 1) # midpoint of rotation angle, direction, point = tff.rotation_from_matrix(matrix_rel) matrix_rel_rot_mid = tff.rotation_matrix( angle * weight_of_new_point, direction, point) # midpoint of translation trans_rel = tff.translation_from_matrix(matrix_rel) matrix_rel_pos_mid = tff.translation_matrix( [x * weight_of_new_point for x in trans_rel]) # relative transformation for midpoint from pointFused matrix_rel_mid = matrix_rel_pos_mid.dot(matrix_rel_rot_mid) # base -> pointFused_new matrix_fused = matrix_rel_mid.dot(matrix_fused) n_fused += 1 # ------------------------------------------------------------------------- assert n_fused == len(df) print('%s Average %s' % ('-' * 30, '-' * 30)) print('N fused: %d' % n_fused) print('Matrix: \n%s' % matrix_fused) trans = tff.translation_from_matrix(matrix_fused) print('Position: {}'.format(trans)) pos_keys = ['position_%s' % x for x in 'xyz'] print('Position (simple): {}'.format(df.mean()[pos_keys].values)) euler = tff.euler_from_matrix(matrix_fused) print('Orientation: {}'.format(euler)) # Compute variance of transformation matrix # ========================================= N = len(df) keys = ['x', 'y', 'z', 'angle'] variance = {k: 0 for k in keys} for index, row in df.iterrows(): # base -> pointIn (sensed calibboard pose) pos = row['position_x'], row['position_y'], row['position_z'] matrix_pos = tff.translation_matrix(pos) rot = row['quaternion_x'], row['quaternion_y'], row['quaternion_z'], row['quaternion_w'] matrix_rot = tff.quaternion_matrix(rot) matrix_pose = matrix_pos.dot(matrix_rot) # pointFused -> pointIn = pointFused -> base -> pointIn matrix_rel = matrix_pose.dot(tff.inverse_matrix(matrix_fused)) # compute distance for translation/rotation delta_x, delta_y, delta_z = tff.translation_from_matrix(matrix_rel) delta_angle, _, _ = tff.rotation_from_matrix(matrix_rel) variance['x'] += delta_x ** 2 variance['y'] += delta_y ** 2 variance['z'] += delta_z ** 2 variance['angle'] += delta_angle ** 2 for k in keys: variance[k] /= (N - 1) print('%s Std Deviation (Variance) %s' % ('-' * 22, '-' * 22)) for k in keys: print('%s: %f (%f)' % (k, variance[k], math.sqrt(variance[k]))) if k != 'angle': print('{} (simple): {}'.format(k, df.std()['position_%s' % k]))
def main(): rospack = rospkg.RosPack() pkg_path = rospack.get_path('hrl_base_selection') #bag_path = '/home/ari/svn/robot1_data/2011_ijrr_phri/log_data/bag/' bag_path = '/home/ari/git/gt-ros-pkg.hrl-assistive/hrl_base_selection/data/yogurt/bags/' bag_name_list = ['2014-06-17-16-13-46', #'2014-06-17-16-17-20', This is a bad bag '2014-06-17-16-19-02', '2014-06-17-16-21-57', '2014-06-17-16-24-03', '2014-06-17-16-26-12', '2014-06-17-16-27-46', '2014-06-17-16-29-18', '2014-06-17-16-31-24', '2014-06-17-16-33-13', '2014-06-18-12-37-23', '2014-06-18-12-39-43', '2014-06-18-12-41-42', '2014-06-18-12-43-45', '2014-06-18-12-45-26', '2014-06-18-12-47-22', '2014-06-18-12-49-04', '2014-06-18-12-50-52', '2014-06-18-12-52-39', '2014-06-18-12-54-26'] # bag_name = '2014-06-17-16-17-20' # bag_name = '2014-06-17-16-19-02' # bag_name = # bag_name = # bag_name = # bag_name = # bag_name = # bag_name = # bag_name = # bag_name = # bag_name = for num,bag_name in enumerate(bag_name_list): print 'Going to open bag: ',bag_name,'.bag' print 'Starting on a tool log file! \n' bag = rosbag.Bag(''.join([bag_path,bag_name,'.bag']), 'r') tool = open(''.join([pkg_path,'/data/yogurt/logs/',bag_name,'_tool.log']), 'w') for topic, msg, t in bag.read_messages(): if topic == "/spoon/pose": tx = copy.copy(msg.transform.translation.x) ty = copy.copy(msg.transform.translation.y) tz = copy.copy(msg.transform.translation.z) rx = copy.copy(msg.transform.rotation.x) ry = copy.copy(msg.transform.rotation.y) rz = copy.copy(msg.transform.rotation.z) rw = copy.copy(msg.transform.rotation.w) world_B_spoon = createBMatrix([tx,ty,tz],[rx,ry,rz,rw])#*adl2_B_pr2goal eul = tft.euler_from_matrix(world_B_spoon,'rxyz') #print world_B_shaver,'\n' #print eul,'\n' #print 'time: \n',t tool.write(''.join([str(t),' %f %f %f %f %f %f \n' % (world_B_spoon[0,3],world_B_spoon[1,3],world_B_spoon[2,3],eul[0],eul[1],eul[2])])) #tool.write(''.join([world_B_shaver[0,3],' ',world_B_shaver[1,3],' ',world_B_shaver[2,3],' ',eul[0],' ',eul[1],' ',eul[2],'\n'])) bag.close() tool.close() print 'Starting on a head log file! \n' bag = rosbag.Bag(''.join([bag_path,bag_name,'.bag']), 'r') head = open(''.join([pkg_path,'/data/yogurt/logs/',bag_name,'_head.log']), 'w') for topic, msg, t in bag.read_messages(): if topic == "/head/pose": tx = copy.copy(msg.transform.translation.x) ty = copy.copy(msg.transform.translation.y) tz = copy.copy(msg.transform.translation.z) rx = copy.copy(msg.transform.rotation.x) ry = copy.copy(msg.transform.rotation.y) rz = copy.copy(msg.transform.rotation.z) rw = copy.copy(msg.transform.rotation.w) world_B_head = createBMatrix([tx,ty,tz],[rx,ry,rz,rw])#*adl2_B_pr2goal eul = copy.copy(tft.euler_from_matrix(world_B_head,'rxyz')) head.write(''.join([str(t),' %f %f %f %f %f %f \n' % (world_B_head[0,3],world_B_head[1,3],world_B_head[2,3],eul[0],eul[1],eul[2])])) #head.write(''.join([t,' ',world_B_head[0,3],' ',world_B_head[1,3],' ',world_B_head[2,3],' ',eul[0],' ',eul[1],' ',eul[2],' ',eul[3],'\n'])) bag.close() head.close() print "Saved file! Finished bag #",num+1 print 'Done with all bag files!'
def start_recording(self): print "Recorder Loop" while self.left_image is None or self.right_image is None: pass if self.record_kinematics: while 1: try: (trans, rot) = self.listener.lookupTransform("/r_gripper_tool_frame", "/base_link", rospy.Time(0)) break except (tf.ExtrapolationException): print "ExtrapolationException" rospy.sleep(0.1) continue frm = 0 wait_thresh = 0 prev_r_l = self.r_l prev_r_r = self.r_r trans_vel = np.array([0.0, 0.0, 0.0]) rot_vel = np.array([0.0, 0.0, 0.0]) prev_trans = None prev_rot = None for i in range(9999999): print frm rospy.sleep(self.period) start = time.time() cv2.imwrite( self.video_folder + self.task_name + "_" + self.trial_name + "_capture1/" + str(get_frame_fig_name(frm)), self.left_image, ) cv2.imwrite( self.video_folder + self.task_name + "_" + self.trial_name + "_capture2/" + str(get_frame_fig_name(frm)), self.right_image, ) if self.record_kinematics: (trans, quaternion) = self.listener.lookupTransform( "/r_gripper_tool_frame", "/base_link", rospy.Time(0) ) r_matrix = utils.quaternion2rotation(quaternion) rot = transformations.euler_from_matrix(r_matrix) r_gripper_angle = self.joint_state.position[-17] if frm != 0: trans_vel = (trans - prev_trans) / self.period rot_vel = (rot - prev_rot) / self.period prev_trans = np.array(trans) prev_rot = np.array(rot) js_pos = self.joint_state.position[16:-12] js_vel = self.joint_state.velocity[16:-12] W = list(trans) + list(r_matrix.flatten()) + list(trans_vel) + list(rot_vel) # Gripper angle is r_gripper_joint W.append(r_gripper_angle) W = W + list(js_pos) + list(js_vel) self.data = utils.safe_concatenate(self.data, utils.reshape(np.array(W))) frm += 1 if (self.r_l == prev_r_l) and (self.r_r == prev_r_r): print "Not recording anymore?" wait_thresh += 1 if wait_thresh > 5: self.save_and_quit() prev_r_l = self.r_l prev_r_r = self.r_r end = time.time() print end - start
plane_resp.pose.orientation.w])) #plane_pose = numpy.eye(4) plane_pose[0,3] = plane_resp.pose.position.x plane_pose[1,3] = plane_resp.pose.position.y plane_pose[2,3] = plane_resp.pose.position.z timeout = 10 destination_frame = '/map' detection_frame = '/head/kinect2_rgb_optical_frame' listener = tf.TransformListener() listener.waitForTransform(detection_frame,destination_frame,rospy.Time(0),rospy.Duration(timeout)) frame_trans,frame_rot = listener.lookupTransform(destination_frame,detection_frame,rospy.Time(0)) frame_offset = numpy.matrix(quaternion_matrix(frame_rot)) frame_offset[0,3] = frame_trans[0] frame_offset[1,3] = frame_trans[1] frame_offset[2,3] = frame_trans[2] plane_in_world = numpy.array(numpy.dot(frame_offset,plane_pose)) #Add pi/2 to az curr_rot = plane_in_world[0:3,0:3] ax,ay,az = euler_from_matrix(curr_rot) curr_rot = euler_matrix(ax,ay,az+numpy.pi/2) plane_in_world[0:3,0:3] = curr_rot[0:3,0:3] env.AddKinBody(plane) plane.SetTransform(plane_in_world)
def move_velocity(self, velocity=0.015, is_translation=True, velocity_rot=None): if velocity_rot is not None: self.update_velocity_rot(velocity_rot) pos_i_des, rot_i_des = self.arm.get_ep() pos_i_act, rot_i_act = self.arm.get_end_effector_pose() # this is the current residiual error in the controller at the start pos_err = pos_i_act - pos_i_des rot_err = rot_i_act.T * rot_i_des if is_translation: pos_vel_des = velocity pid_ctrl = PIDController(rate=rate, k_p=kp_proportional, k_i=kp_integral, i_max=None, init_integ=np.sign(pos_vel_des) * kp_init_integral, saturation=kp_max_ctrl, feed_forward=np.sign(pos_vel_des) * kp_constant, name="arm_vel") else: rot_vel_des = velocity pid_ctrl = PIDController(rate=rate, k_p=kr_proportional, k_i=kr_integral, i_max=None, init_integ=np.sign(rot_vel_des) * kr_init_integral, saturation=kr_max_ctrl, feed_forward=np.sign(rot_vel_des) * kr_constant, name="arm_vel") vels = deque([np.array([0]*6)]*40) r = rospy.Rate(rate) self._is_moving = True while not rospy.is_shutdown() and self._is_moving: with self.lock: vel_rot = self.velocity_rot.copy() cur_pos, cur_rot = self.arm.get_end_effector_pose() # hacky velocity filter xd_act = self.arm.get_controller_state()['xd_act'] vels.append(xd_act) vels.popleft() vel_filt = np.mat(np.mean(vels, 0)).T x_vel_filt = (vel_rot.T * vel_filt[:3,0])[0,0] roll_vel_filt = (vel_rot.T * vel_filt[3:,0])[0,0] if is_translation: # PI velocity controller for position pos_ctrl = pid_ctrl.update_state(pos_vel_des - x_vel_filt) pos_des = vel_rot * (np.mat([pos_ctrl, 0, 0]).T + np.mat(np.diag([1, 0, 0])) * vel_rot.T * (cur_pos - pos_err) + np.mat(np.diag([0, 1, 1])) * vel_rot.T * pos_i_des) rot_des = rot_i_des # don't change rotation if not is_translation: rot_des_vel_frame = np.mat(np.eye(4)) rot_des_vel_frame[:3,:3] = cur_rot * rot_err * vel_rot roll_des_vel_frame, r2, r3 = tf_trans.euler_from_matrix(rot_des_vel_frame) # PI velocity controller for rotation rot_ctrl = pid_ctrl.update_state(rot_vel_des + roll_vel_filt) print roll_vel_filt, rot_vel_des, rot_vel_des - roll_vel_filt roll_ctrl_des = roll_des_vel_frame + rot_ctrl r1, pitch_i_des, yaw_i_des = tf_trans.euler_from_matrix(rot_i_des * vel_rot) rot_des = np.mat(tf_trans.euler_matrix(roll_ctrl_des, pitch_i_des, yaw_i_des)[:3,:3]) * vel_rot.T pos_des = pos_i_des # don't change translation self.arm.set_ep((pos_des, rot_des), 1) r.sleep() self.arm.set_ep(self.arm.get_ep(), 1)
def quaternion_to_rpy(q): 'Take a Quaternion message and return a numpy rpy vector.' q_arr = np.array([q.x, q.y, q.z, q.w]) R = transformations.quaternion_matrix(q_arr) return transformations.euler_from_matrix(R, axes='sxyz')
def _make_generic(args): if type(args[0]) == str: frame_id = args[0] header, homo_mat, rot_quat, rot_euler = PoseConverter._make_generic(args[1:]) if header is None: header = [0, rospy.Time.now(), ''] header[2] = frame_id return header, homo_mat, rot_quat, rot_euler if len(args) == 1: if type(args[0]) is Pose: homo_mat, rot_quat, rot_euler = PoseConverter._extract_pose_msg(args[0]) return None, homo_mat, rot_quat, rot_euler elif type(args[0]) is PoseStamped: homo_mat, rot_quat, rot_euler = PoseConverter._extract_pose_msg(args[0].pose) seq = args[0].header.seq stamp = args[0].header.stamp frame_id = args[0].header.frame_id return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler elif type(args[0]) is Transform: homo_mat, rot_quat, rot_euler = PoseConverter._extract_tf_msg(args[0]) return None, homo_mat, rot_quat, rot_euler elif type(args[0]) is TransformStamped: homo_mat, rot_quat, rot_euler = PoseConverter._extract_tf_msg(args[0].transform) seq = args[0].header.seq stamp = args[0].header.stamp frame_id = args[0].header.frame_id return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler elif type(args[0]) is Twist: homo_mat, rot_quat, rot_euler = PoseConverter._extract_twist_msg(args[0]) return None, homo_mat, rot_quat, rot_euler elif type(args[0]) is TwistStamped: homo_mat, rot_quat, rot_euler = PoseConverter._extract_twist_msg(args[0].twist) seq = args[0].header.seq stamp = args[0].header.stamp frame_id = args[0].header.frame_id return [seq, stamp, frame_id], homo_mat, rot_quat, rot_euler elif isinstance(args[0], (np.matrix, np.ndarray)) and np.shape(args[0]) == (4, 4): return (None, np.mat(args[0]), tf_trans.quaternion_from_matrix(args[0]).tolist(), tf_trans.euler_from_matrix(args[0])) elif isinstance(args[0], (tuple, list)) and len(args[0]) == 2: pos_arg = np.mat(args[0][0]) rot_arg = np.mat(args[0][1]) if pos_arg.shape == (1, 3): # matrix is row, convert to column pos = pos_arg.T elif pos_arg.shape == (3, 1): pos = pos_arg else: return None, None, None, None if rot_arg.shape == (3, 3): # rotation matrix homo_mat = np.mat(np.eye(4)) homo_mat[:3,:3] = rot_arg quat = tf_trans.quaternion_from_matrix(homo_mat) rot_euler = tf_trans.euler_from_matrix(homo_mat) else: if rot_arg.shape[1] == 1: # make into row matrix rot_arg = rot_arg.T else: rot_arg = rot_arg.tolist()[0] if len(rot_arg) == 3: # Euler angles rotation homo_mat = np.mat(tf_trans.euler_matrix(*rot_arg)) quat = tf_trans.quaternion_from_euler(*rot_arg) rot_euler = rot_arg elif len(rot_arg) == 4: # quaternion rotation homo_mat = np.mat(tf_trans.quaternion_matrix(rot_arg)) quat = rot_arg rot_euler = tf_trans.euler_from_quaternion(quat) else: return None, None, None, None homo_mat[:3, 3] = pos return None, homo_mat, np.array(quat), rot_euler elif len(args) == 2: header, homo_mat, rot_quat, rot_euler = PoseConverter._make_generic( ((args[0], args[1]),)) if homo_mat is not None: return header, homo_mat, rot_quat, rot_euler return None, None, None, None
def _turn_by(self, delta_ang, block=True): current_ang_odom = tr.euler_from_matrix(tfu.transform('base_footprint',\ 'odom_combined', self.tflistener)[0:3, 0:3], 'sxyz')[2] self.turn_to(current_ang_odom + delta_ang, block)
def DetectBlocks(self, robot, table, blocks=None,timeout=10, **kw_args): """ Calls detector for blocks and places them on the table @param robot: The robot instance using the detector @param table: The kinbody for the table on which the blocks are placed @blocks: List of blocks currently in the environment; if present, redetection not done @return The list of blocks detected """ if blocks is not None and len(blocks) == 0: # Add all blocks env = robot.GetEnv() # Get the pr-ordata package path import prpy.util block_path = os.path.join('objects', 'block.kinbody.xml') detected_blocks = self.find_blocks(cloud_topic=self.point_cloud_topic) # Place blocks on the table from prpy.util import ComputeEnabledAABB with prpy.rave.Disabled(table, padding_only=True): table_aabb = ComputeEnabledAABB(table) z = table_aabb.pos()[2] + table_aabb.extents()[2] + table_z_offset #OFFSET SET AT TOP for b in detected_blocks: block = env.ReadKinBodyXMLFile(block_path) for link in block.GetLinks(): for geometry in link.GetGeometries(): geometry.SetDiffuseColor(numpy.array([b.avg_color.r,b.avg_color.g,b.avg_color.b,b.avg_color.a])) block_pose = numpy.array(quaternion_matrix([ b.pose.orientation.x, b.pose.orientation.y, b.pose.orientation.z, b.pose.orientation.w])) block_pose[0,3] = b.pose.position.x block_pose[1,3] = b.pose.position.y block_pose[2,3] = b.pose.position.z self.listener.waitForTransform( self.detection_frame, self.destination_frame, rospy.Time(0), rospy.Duration(timeout)) frame_trans, frame_rot = self.listener.lookupTransform( self.destination_frame, self.detection_frame, rospy.Time(0)) frame_offset = numpy.matrix(quaternion_matrix(frame_rot)) frame_offset[0,3] = frame_trans[0] frame_offset[1,3] = frame_trans[1] frame_offset[2,3] = frame_trans[2] block_in_world = numpy.array(numpy.dot(frame_offset,block_pose)) #Snap block to table block_in_world[2,3] = z #To snap blocks to upright on table block_matrix = block_in_world[0:3,0:3] ax, ay, az = euler_from_matrix(block_matrix) ax = 0 ay = 0 block_in_world_corrected = euler_matrix(ax,ay,az) block_in_world[0:3,0:3] = block_in_world_corrected[0:3,0:3] block.SetTransform(block_in_world) #Set block name block.SetName('block') env.Add(block,anonymous=True) blocks.append(block) return blocks