def get_angles_between_two_vector(self, vec1, vec2): vec1_unit = tf.unit_vector(vec1) vec2_unit = tf.unit_vector(vec2) inner_product = np.dot(vec1_unit, vec2_unit) theta = m.acos(inner_product) degrees = m.degrees(theta) return abs(degrees)
def find_mesh_holes(vert, faces, radius, scale=1., fitplane_eps=1e-8, fitplane_attempts=10): vertices = np.array(vert) * scale # Circles have lots of vertices. Use clustering to locate them eps = radius + 1e-3 db = sklearn.cluster.DBSCAN(eps=eps, min_samples=10).fit(vertices) unique_labels = set(db.labels_) circles_info = [] for k in unique_labels: if k == -1: # Unknown cluster continue points = vertices[db.labels_ == k] for _ in range(fitplane_attempts): seed = np.zeros(4) seed[:3] = tr.unit_vector(tr.random_vector(3)) res = criros.spalg.fit_plane_optimize(points, seed=seed) equation = res[0] fit_error = res[2] if fit_error < fitplane_eps: break data = dict() data['center'] = np.mean(points, axis=0) data['plane'] = criros.spalg.Plane(equation=res[0]) circles_info.append(data) if fit_error > fitplane_eps: # Report circles that weren't fitted properly print 'Circle planefit error above threshold: {0}'.format( fit_error) # One hole is composed by two circles, pair them holes = [] num_circles = len(circles_info) found = set() for i in range(num_circles): if i in found: continue # Skip already paired circles for j in range(1, num_circles): if (j in found) or (i == j): continue # Skip already paired circles plane_i = circles_info[i]['plane'] plane_j = circles_info[j]['plane'] ni = plane_i.normal nj = plane_j.normal parallel = np.isclose(abs(np.dot(ni, nj)), 1.) center_i = circles_info[i]['center'] center_j = circles_info[j]['center'] pi = center_i pj = plane_i.project(center_j) if parallel and np.allclose(pi, pj): found.add(i) found.add(j) position = center_j diff = center_i - center_j direction = tr.unit_vector(diff) depth = np.linalg.norm(diff) holes.append(Hole(position, direction, depth)) return holes
def enu_from_ecef_tf(ecef_pos): up_ecef = transformations.unit_vector(ecef_from_latlongheight( *latlongheight_from_ecef(ecef_pos) + numpy.array([0, 0, 1])) - ecef_pos) east_ecef = transformations.unit_vector(numpy.cross([0, 0, 1], up_ecef)) north_ecef = numpy.cross(up_ecef, east_ecef) enu_from_ecef = numpy.array([east_ecef, north_ecef, up_ecef]) return enu_from_ecef
def execute(self, ud): marker_pose = ud.marker_pose # type: PoseStamped marker_position = np.array( [marker_pose.pose.position.x, marker_pose.pose.position.y]) r_mo = transformations.unit_vector(marker_position - self.origin) goal_position = marker_position + self.back_distance * r_mo e_mo = transformations.unit_vector(np.append(r_mo, [0])) orientation_facing_marker = np.eye(4) orientation_facing_marker[0:3, 0:3] = np.column_stack( (-e_mo, np.cross([0, 0, 1], -e_mo), [0, 0, 1])) orientation_facing_marker = transformations.quaternion_from_matrix( orientation_facing_marker) goal_pose = PoseStamped() goal_pose.header.frame_id = marker_pose.header.frame_id goal_pose.pose.position = Point(goal_position[0], goal_position[1], 0) goal_pose.pose.orientation = Quaternion( orientation_facing_marker[0], orientation_facing_marker[1], orientation_facing_marker[2], orientation_facing_marker[3], ) # goal_pose.pose.orientation = Quaternion(0, 0, 0, 1) ud.target_pose = goal_pose return 'ok'
def look_at(direction, up): z = tfs.unit_vector(direction) x = tfs.unit_vector(np.cross(up, z)) y = np.cross(z, x).reshape(-1, 1) x = x.reshape(-1, 1) z = z.reshape(-1, 1) return np.hstack((x, y, z))
def execute(self, ud): cube_pose = self.cube.pose # type: PoseStamped cube_position = np.array( [cube_pose.pose.position.x, cube_pose.pose.position.y]) tag_pose = self.target() tag_position = np.array( [tag_pose.pose.position.x, tag_pose.pose.position.y]) r_mo = transformations.unit_vector(cube_position - tag_position) goal_position = cube_position + self.back_distance * r_mo goal_point = closest_square( Point(goal_position[0], goal_position[1], 0.), self.squares).pose.pose.position e_mo = transformations.unit_vector(np.append(r_mo, [0])) orientation_facing_marker = np.eye(4) orientation_facing_marker[0:3, 0:3] = np.column_stack( (-e_mo, np.cross([0, 0, 1], -e_mo), [0, 0, 1])) orientation_facing_marker = transformations.quaternion_from_matrix( orientation_facing_marker) goal_pose = PoseStamped() goal_pose.header.frame_id = cube_pose.header.frame_id goal_pose.pose.position = goal_point goal_pose.pose.orientation = Quaternion( orientation_facing_marker[0], orientation_facing_marker[1], orientation_facing_marker[2], orientation_facing_marker[3], ) ud.target_pose = goal_pose return 'ok'
def residuals(x): assert len(x) == 4 pos = x[0:3] t = x[3] / c import glonass # for inertial_from_ecef # for sat in sats: # print sat.prn, (sat.T_iono + sat.T_tropo if use_corrections else 0)*c return [ numpy.linalg.norm( glonass.inertial_from_ecef(t, pos) - glonass.inertial_from_ecef(sat.time, xyz_array(sat.position)) ) - (t - sat.time - (sat.T_iono + sat.T_tropo if use_corrections else 0)) * c for sat in sats], numpy.array([[transformations.unit_vector( glonass.inertial_from_ecef(t, pos) - glonass.inertial_from_ecef(sat.time, xyz_array(sat.position)) ).dot(glonass.inertial_from_ecef(t, [1, 0, 0])), transformations.unit_vector( glonass.inertial_from_ecef(t, pos) - glonass.inertial_from_ecef(sat.time, xyz_array(sat.position)) ).dot(glonass.inertial_from_ecef(t, [0, 1, 0])), transformations.unit_vector( glonass.inertial_from_ecef(t, pos) - glonass.inertial_from_ecef(sat.time, xyz_array(sat.position)) ).dot(glonass.inertial_from_ecef(t, [0, 0, 1])), transformations.unit_vector( glonass.inertial_from_ecef(t, pos) - glonass.inertial_from_ecef(sat.time, xyz_array(sat.position)) ).dot(glonass.inertial_vel_from_ecef_vel(t, [0, 0, 0], pos)) / c - 1] for sat in sats])
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 pose_point_at(position, z_axis, rough_x_axis): # type: (PointLike, PointLike, PointLike) -> Pose tmp_x_dir = unit_vector(pointlike_to_array(rough_x_axis)) z_dir = unit_vector(pointlike_to_array(z_axis)) y_dir = unit_vector(np.cross(z_dir, tmp_x_dir)) x_dir = unit_vector(np.cross(y_dir, z_dir)) matrix = construct_transform_matrix(position, x_dir, y_dir, z_dir) return matrix_to_pose(matrix)
def __init__(self, normal=None, point=None, equation=None): if equation is None: self.point = np.array(point) self.normal = tr.unit_vector(normal) self.offset = -np.dot(self.normal, self.point) else: norm = tr.vector_norm(equation[:3]) self.normal = tr.unit_vector(equation[:3]) self.offset = equation[3] / norm self.point = -self.offset*self.normal # Plane origin self.origin = np.array(self.point)
def rotation_matrix_from_axes(newaxis, oldaxis=Z_AXIS): """ Return the rotation matrix that aligns two vectors. """ oldaxis = tr.unit_vector(oldaxis) newaxis = tr.unit_vector(newaxis) c = np.dot(oldaxis, newaxis) angle = np.arccos(c) if np.isclose(c, -1.0) or np.allclose(newaxis, oldaxis): v = perpendicular_vector(newaxis) else: v = np.cross(oldaxis, newaxis) return tr.rotation_matrix(angle, v)
def look_at_general(eye, center, up): # f = tfs.unit_vector(center - eye) f = tfs.unit_vector(center) u = tfs.unit_vector(up) s = tfs.unit_vector(np.cross(u, f)) u = np.cross(s, f) result = np.eye(4) result[0:3, 0] = -s result[0:3, 1] = -f result[0:3, 2] = u result[0:3, 3] = eye return result
def look_at_mat(rel_pos): forward = transformations.unit_vector(rel_pos) left = transformations.unit_vector(numpy.cross([0, 0, 1], forward)) up = numpy.cross(forward, left) mat = numpy.array([ -left, -up, forward, ]) return numpy.vstack([ numpy.hstack([mat, [[0], [0], [0]]]), [[0, 0, 0, 1]], ])
def fit_plane_optimize(points, seed=None): """ Fits a plane to a point cloud using C{scipy.optimize.leastsq} @type points: list @param points: list of points @rtype: np.array @return: normalized normal vector of the plane """ if seed is None: seed = np.zeros(4) seed[:3] = tr.unit_vector(tr.random_vector(3)) # Optimization functions def f_min(X,p): normal = p[0:3] d = p[3] result = ((normal*X.T).sum(axis=1) + d) / np.linalg.norm(normal) return result def residuals(params, signal, X): return f_min(X, params) # Optimize XYZ = np.array(points).T p0 = np.array(seed) sol = scipy.optimize.leastsq(residuals, p0, args=(None, XYZ))[0] nn = np.linalg.norm(sol[:3]) sol /= nn seed_error = (f_min(XYZ, p0)**2).sum() fit_error = (f_min(XYZ, sol)**2).sum() return sol, seed_error, fit_error
def quat_to_rotvec(q): if q[3] < 0: q = -q q = transformations.unit_vector(q) angle = np.arccos(q[3]) * 2 axis = normalized(q[0:3]) return axis * angle
def random_lookat_ray(goal, radius, variance, fov): """ This function returns a random point and the direction from the point to the given goal. The random point is inside a chopped upper sphere. @type goal : np.array @param goal : homogeneous transformation of the goal to look at @type radius : float @param radius : minimum radius of the sphere in meters @type variance : float @param variance : variance of the radius in meters @rtype: orpy.Ray @return: The random Ray that looks towards the goal """ theta1 = 2. * np.pi * np.random.uniform(-fov, fov) theta2 = np.arccos(1 - np.random.uniform(0, fov)**2) r = radius + variance * np.random.uniform(0, 1.) x = r * np.cos(theta1) * np.sin(theta2) y = r * np.sin(theta1) * np.sin(theta2) z = r * np.cos(theta2) R = goal[:3, :3] point = goal[:3, 3] + np.dot(R, np.array([x, y, z])) # Find the direction direction = -np.dot(R, np.array([x, y, z])) direction = tr.unit_vector(direction) return orpy.Ray(point, direction)
def cal_rpy(self, z_axis): ref_axis = np.array([0, 0, 1]) target_axis = z_axis target_axis_unit = tf.unit_vector(z_axis) axis_inner_product = np.dot(ref_axis, target_axis_unit) target_theta = m.acos(axis_inner_product) if abs(axis_inner_product) != 1: cross_axis = np.cross(ref_axis, target_axis_unit) cross_axis_unit = tf.unit_vector(cross_axis) print(cross_axis_unit) rot_matrix = self.get_rot_mat_by_principalAxis(cross_axis_unit, target_theta) rpy = np.array(list(tf.euler_from_matrix(rot_matrix))) rpy = np.nan_to_num(rpy) return rpy else: return np.array([0, target_theta, 0])
def convert_quat_to_quat_frame(quat, relative_trans_quat): ''' Given the quaternion that defines the transformation from its frame of reference to the target frame of reference, transform a quaternion to the target frame @param quat - the quaternion to be transformed. List-like (x, y, z, w) @param relative_trans_quat - the quaternion that defines the transformation from quat's frame to the target frame @return the converted quaternion, tuple (x, y, z, w) ''' # We want to transform quat from Frame1 to Frame2 # then if Frame1 --q-> Frame2, relative_trans_quat is q-inverse relative_trans_quat = unit_vector(relative_trans_quat) relative_trans_inv = (relative_trans_quat[0], relative_trans_quat[1], relative_trans_quat[2], -relative_trans_quat[3]) x_qinv = unit_vector(quaternion_multiply(quat, relative_trans_inv)) return unit_vector(quaternion_multiply(relative_trans_quat, x_qinv))
def qv_mult(q1, v1): v1 = transformations.unit_vector(v1) q2 = list(v1) q2.append(0.0) return transformations.quaternion_multiply( transformations.quaternion_multiply(q1, q2), transformations.quaternion_conjugate(q1))[:3]
def random_lookat_ray(goal, radius, variance, fov): """ This function returns a random point and the direction from the point to the given goal. The random point is inside a chopped upper sphere. @type goal : np.array @param goal : homogeneous transformation of the goal to look at @type radius : float @param radius : minimum radius of the sphere in meters @type variance : float @param variance : variance of the radius in meters @rtype: orpy.Ray @return: The random Ray that looks towards the goal """ theta1 = 2.*np.pi*np.random.uniform(-fov, fov) theta2 = np.arccos(1 - np.random.uniform(0, fov)**2) r = radius + variance*np.random.uniform(0,1.) x = r*np.cos(theta1)*np.sin(theta2) y = r*np.sin(theta1)*np.sin(theta2) z = r*np.cos(theta2) R = goal[:3,:3] point = goal[:3,3] + np.dot(R, np.array([x,y,z])) # Find the direction direction = -np.dot(R, np.array([x,y,z])) direction = tr.unit_vector(direction) return orpy.Ray(point, direction)
def transformPoseToList(msg): quat = tf.unit_vector( np.array([ msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ])) yaw, pitch, roll = tf.euler_from_quaternion(quat, 'rzyx') return [msg.position.x, msg.position.y, msg.position.z, roll, pitch, yaw]
def interpolate(q0, q1, beta): if np.linalg.norm(q0) < _EPS: return q1 q0 = unit_vector(q0) q1 = unit_vector(q1) if beta == 0.0: return q0 d = np.dot(q0, q1) angle = np.arccos(d) if abs(angle) < _EPS: return q0 fraction = beta / angle isin = 1.0 / np.sin(angle) q0 *= np.sin((1.0 - fraction) * angle) * isin q1 *= np.sin(fraction * angle) * isin q0 += q1 return q0
def move_out_of_collision(env, body, max_displacement=0.005): """ Moves an OpenRAVE body out of collision in the opposite direction to the penetration direction. @type env: orpy.Environment @param env: The OpenRAVE environment. @type body: orpy.KinBody @param body: The OpenRAVE body. @type max_displacement: float @param max_displacement: The maximum displacement we can apply to the body. """ if not env.CheckCollision(body): # Not in collision return True # Need to use pqp collision checker previous_cc = env.GetCollisionChecker() checker = orpy.RaveCreateCollisionChecker(env, 'pqp') checker.SetCollisionOptions(orpy.CollisionOptions.Distance | orpy.CollisionOptions.Contacts) env.SetCollisionChecker(checker) # Collision report report = orpy.CollisionReport() env.CheckCollision(body, report) # Restore previous collision checker env.SetCollisionChecker(previous_cc) # Get the direction we should push the object positions = [] normals = [] occurrences = [] for c in report.contacts: positions.append(c.pos) if len(normals) == 0: normals.append(c.norm) occurrences.append(1) continue found = False for i, normal in enumerate(normals): if np.allclose(c.norm, normal): occurrences[i] += 1 found = True break if not found: normals.append(c.norm) occurrences.append(1) push_direction = tr.unit_vector(normals[np.argmax(occurrences)]) # Get the distance we should push the object Tbody = body.GetTransform() Tnew = np.array(Tbody) push_distance = 0 while env.CheckCollision(body): push_distance += 0.001 Tnew[:3, 3] = Tbody[:3, 3] + push_distance * push_direction body.SetTransform(Tnew) if push_distance > max_displacement: print 'push_distance: {0}'.format(push_distance) body.SetTransform(Tbody) return False return True
def store_quaternion_data(self, quaternion_data): self._quaternion = copy.deepcopy(quaternion_data) self._unit_imu_quaternion = tfms.unit_vector([ self._quaternion.quaternion.x / 1000, self._quaternion.quaternion.y / 1000, self._quaternion.quaternion.z / 1000, self._quaternion.quaternion.w / 1000 ])
def direction_from_quaternion(orientation, axis=2): # type: (Quaternion, int) -> np.ndarray # 'z-axis' of the rotation matrix represents the 'forward' axis matrix = quaternion_matrix(quaternion_to_array(orientation)) axis = max(0, min(2, axis)) direction = matrix[0:3, axis] return unit_vector(direction)
def transformPoseToList_quat(msg,topic_name): """ Transform a pose message into an array of xyz, quaternion in xyzw format """ if topic_name == '/vins_estimator/camera_pose': quat = tf.unit_vector(np.array([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])) return [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z, quat[0], quat[1], quat[2], quat[3]] elif topic_name == '/vrpn_client/vins/pose': quat = tf.unit_vector(np.array([msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z, msg.transform.rotation.w])) return [msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z, quat[0], quat[1], quat[2], quat[3]] else: assert False, "topic name '%r' not supported" % topic_name
def rotation_matrix_from_axes(newaxis, oldaxis=Z_AXIS): """ Returns the rotation matrix that aligns two vectors. @type newaxis: np.array @param newaxis: The goal axis @type oldaxis: np.array @param oldaxis: The initial axis @rtype: array, shape (3,3) @return: The resulting rotation matrix that aligns the old to the new axis. """ oldaxis = tr.unit_vector(oldaxis) newaxis = tr.unit_vector(newaxis) c = np.dot(oldaxis, newaxis) angle = np.arccos(c) if np.isclose(c, -1.0) or np.allclose(newaxis, oldaxis): v = perpendicular_vector(newaxis) else: v = np.cross(oldaxis, newaxis) return tr.rotation_matrix(angle, v)
def update_quat_omg_dt(quat, omg, dt): ox, oy, oz = omg[0], omg[1], omg[2] qw, qx, qy, qz = quat[3], quat[0], quat[1], quat[2] dqw = -0.5 * (ox * qx + oy * qy + oz * qz) dqx = 0.5 * (ox * qw + oy * qz - oz * qy) dqy = 0.5 * (oy * qw + oz * qx - ox * qz) dqz = 0.5 * (oz * qw + ox * qy - oy * qx) q = [qx + dqx * dt, qy + dqy * dt, qz + dqz * dt, qw + dqw * dt] return unit_vector(q)
def euler_to_quat_list(data): euler_list = data[:,3:6] p_list = data[:,0:3] quat_list = [] for rows in euler_list: tmp = quaternion_from_euler(rows[0],rows[1],rows[2]) # tmp = [ round(elem, 4) for elem in tmp ] tmp = unit_vector(tmp) quat_list.append(tmp) stack = np.hstack((p_list,quat_list)) return stack
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 transformPoseToList(msg,topic_name): """ Transform a pose message into an array of xyz, Euler rpy in ZYX format """ if topic_name == '/vins_estimator/camera_pose': quat = tf.unit_vector(np.array([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])) yaw, pitch, roll = tf.euler_from_quaternion(quat, 'rzyx') return [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z, roll, pitch, yaw] elif topic_name == '/vrpn_client/vins/pose': quat = tf.unit_vector(np.array([msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z, msg.transform.rotation.w])) yaw, pitch, roll = tf.euler_from_quaternion(quat, 'rzyx') return [msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z, roll, pitch, yaw] else: assert False, "topic name '%r' not supported" % topic_name
def trajectory_to_wheel_speeds(msg): def get_vel_at_point(body_point): return xyz_array(msg.twist.linear) + numpy.cross(xyz_array(msg.twist.angular), body_point) ws_req = SetWheelSpeedsRequest() [ws_req.wheel1, ws_req.wheel2, ws_req.wheel3, ws_req.wheel4] = [ get_vel_at_point(wheel_pos).dot(transformations.unit_vector(wheel_dir)) / wheel_radius * math.sqrt(2) for wheel_pos, wheel_dir in wheels] set_wheel_speed_service(ws_req)
def trajectory_to_wheel_speeds(msg): def get_vel_at_point(body_point): return xyz_array(msg.twist.linear) + numpy.cross( xyz_array(msg.twist.angular), body_point) ws_req = SetWheelSpeedsRequest() [ws_req.wheel1, ws_req.wheel2, ws_req.wheel3, ws_req.wheel4] = [ get_vel_at_point(wheel_pos).dot(transformations.unit_vector(wheel_dir)) / wheel_radius * math.sqrt(2) for wheel_pos, wheel_dir in wheels ] set_wheel_speed_service(ws_req)
def perpendicular_vector(v): """ Finds an arbitrary perpendicular vector to B{v} """ v = tr.unit_vector(v) if np.allclose(v[:2], np.zeros(2)): if np.isclose(v[2], 0.): # v is (0, 0, 0) raise ValueError('zero vector') # v is (0, 0, Z) return Y_AXIS return np.array([-v[1], v[0], 0])
def translate_pose_relative(pose, t_x=0, t_y=0, t_z=0): # type: (Pose, float, float, float) -> Pose """ Returns a pose by translating the given pose along its relative coordinate axes by the given factors. """ _pose = Pose( Point(pose.position.x, pose.position.y, pose.position.z), Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)) matrix = quaternion_matrix(quaternion_to_array(pose.orientation)) x_dir = unit_vector(matrix[0:3, 0]) y_dir = unit_vector(matrix[0:3, 1]) z_dir = unit_vector(matrix[0:3, 2]) _pose.position.x += t_x * x_dir[0] + t_y * y_dir[0] + t_z * z_dir[0] _pose.position.y += t_x * x_dir[1] + t_y * y_dir[1] + t_z * z_dir[1] _pose.position.z += t_x * x_dir[2] + t_y * y_dir[2] + t_z * z_dir[2] return _pose
def qerror(self, q1, q2): """ Returns rotation vector representing the orientation error from quaternion q1 to q2. """ # Quaternion from q1 to q2 qdiff = trns.quaternion_multiply(q2, trns.quaternion_inverse(q1)) # Renormalize for accuracy qdiff = trns.unit_vector(qdiff) # If "amount of rotation" is negative, flip quaternion if qdiff[3] < 0: qdiff = -qdiff # Extract axis if not np.allclose(qdiff[:3], 0): axis = trns.unit_vector(qdiff[:3]) else: axis = np.array([0, 0, 0]) # Extract angle angle = 2 * np.arccos(qdiff[3]) # Return rotvec return angle * axis
def line(start, end, width=3/4*INCH, color=(255, 255, 255, 255)): start = numpy.array(start) threeify = lambda (x, y): (x, y, 0) perp = numpy.array([ [0, -1], [1, 0], ]).dot(transformations.unit_vector(end-start)) * width/2 r.draw_polygon([ threeify(start+perp), threeify(end+perp), threeify(end-perp), threeify(start-perp), ], color)
def quat_to_rotvec(q): ''' Convert a quaternion to a rotation vector ''' # For unit quaternion, return 0 0 0 if np.all(np.isclose(q[0:3], 0)): return np.array([0., 0., 0.]) if q[3] < 0: q = -q q = trans.unit_vector(q) angle = np.arccos(q[3]) * 2 norm = np.linalg.norm(q) axis = q[0:3] / norm return axis * angle
def rotate_quat_by_omega(quat, omega, dt): omega_mag = tf_math.vector_norm(omega) temp_term = 0.5 * omega_mag * dt psi_vec = sin(temp_term) * omega if omega_mag != 0.: psi_vec = psi_vec / omega_mag omega_mat = eye(4) * cos(temp_term) omega_mat[0:3,3] = psi_vec omega_mat[3,0:3] = -psi_vec full_quat = zeros((4,1)) full_quat[0:3,0] = quat full_quat[3,0] = sqrt(1 - dot(quat, quat)) if not alltrue(isreal(full_quat)): #TODO Should find a better exception class for this raise Exception("Quat not completely real... this is troubling") new_quat = dot(omega_mat, full_quat) new_quat = tf_math.unit_vector(new_quat) return new_quat[0:3,0]
def __init__(self, normal, point): self.point = np.array(point) self.normal = tr.unit_vector(normal) self.offset = -np.dot(self.normal, self.point)
def __init__(self, *args, **kwargs): self._body_vec_align = transformations.unit_vector(kwargs.pop('body_vec_align', [1, 0, 0])) self._done_ctr = 0 BaseManeuverObjectState.__init__(self, *args, **kwargs)
def _rotvec_from_quat(q): q = transformations.unit_vector(q) if q[3] < 0: q = -q return 2 / _sinc(math.acos(min(1, q[3]))) * q[:3]
def move_out_of_collision(env, body, max_displacement=0.005): """ Moves an OpenRAVE body out of collision in the opposite direction to the penetration direction. @type env: orpy.Environment @param env: The OpenRAVE environment. @type body: orpy.KinBody @param body: The OpenRAVE body. @type max_displacement: float @param max_displacement: The maximum displacement we can apply to the body. """ if not env.CheckCollision(body): # Not in collision return True # Need to use pqp collision checker previous_cc = env.GetCollisionChecker() checker = orpy.RaveCreateCollisionChecker(env, 'pqp') checker.SetCollisionOptions(orpy.CollisionOptions.Distance|orpy.CollisionOptions.Contacts) env.SetCollisionChecker(checker) # Collision report report = orpy.CollisionReport() env.CheckCollision(body, report) # Restore previous collision checker env.SetCollisionChecker(previous_cc) # Get the direction we should push the object positions = [] normals = [] occurrences = [] for c in report.contacts: positions.append(c.pos) if len(normals) == 0: normals.append(c.norm) occurrences.append(1) continue found = False for i,normal in enumerate(normals): if np.allclose(c.norm, normal): occurrences[i] += 1 found = True break if not found: normals.append(c.norm) occurrences.append(1) push_direction = tr.unit_vector(normals[np.argmax(occurrences)]) # Get the distance we should push the object hull = scipy.spatial.ConvexHull(positions) vertices = np.array(positions)[hull.vertices] push_distance = -float('inf') for i in range(len(vertices)): p1 = vertices[i] for j in range(i+1, len(vertices)): p2 = vertices[j] dist = np.linalg.norm((p1-p2)*push_direction) if dist > push_distance: push_distance = dist push_distance = push_distance + 0.001 if not (0 < push_distance < max_displacement): print '{0} - push_distance: {0}'.format(body.GetName(), push_distance) return False # Move the object out of collision Tbody = body.GetTransform() Tnew = np.array(Tbody) Tnew[:3,3] += push_distance*push_direction body.SetTransform(Tnew) if env.CheckCollision(body): # Failed to move it out of collision body.SetTransform(Tbody) print 'Failed to move it out of collision' print 'push_distance: {0}'.format(push_distance) print 'push_direction: {0}'.format(push_direction) return False # Restore previous collision checker return True
if last_odom_time is None: last_odom_time = stamp continue dt = (stamp - last_odom_time).to_sec() last_odom_time = stamp #print dt A = [] b = [] def get_vel_at_point(vel, angvel, body_point): assert len(vel) == 2 return numpy.array([vel[0], vel[1], 0]) + numpy.cross(numpy.array([0, 0, angvel]), body_point) for i, (wheel_pos, wheel_dir) in enumerate(wheels): A.append([ get_vel_at_point([1, 0], 0, wheel_pos).dot(transformations.unit_vector(wheel_dir)) / wheel_radius * math.sqrt(2), get_vel_at_point([0, 1], 0, wheel_pos).dot(transformations.unit_vector(wheel_dir)) / wheel_radius * math.sqrt(2), get_vel_at_point([0, 0], 1, wheel_pos).dot(transformations.unit_vector(wheel_dir)) / wheel_radius * math.sqrt(2), ]) b.append(getattr(odom, "wheel%i" % (i+1,))/dt) x, residuals, rank, s = numpy.linalg.lstsq(A, b) vel = x[0:2] angvel = x[2] #print vel, angvel dp, dtheta = drive(vel, angvel, dt) odom_pub.publish(PoseStamped( header=Header(
def enu_from_ecef(v, pos): up_ecef = transformations.unit_vector(pos) east_ecef = transformations.unit_vector(numpy.cross([0, 0, 1], up_ecef)) north_ecef = numpy.cross(up_ecef, east_ecef) enu_from_ecef = numpy.array([east_ecef, north_ecef, up_ecef]) return enu_from_ecef.dot(v)
def __init__(self, logger=rospy): # Generic np.set_printoptions(precision=5, suppress=True) # Read values from the ROS parameter server id_planning = criros.read_parameter('/openrave_ids/planning', 1) id_state = criros.read_parameter('/openrave_ids/state', 2) # Load OpenRAVE environment env = orpy.Environment() orpy.RaveSetDebugLevel(orpy.DebugLevel.Fatal) worldxml = 'worlds/bimanual_ikea_assembly.env.xml' if not env.Load(worldxml): raise Exception('World file does not exist: %s' % worldxml) left_robot = env.GetRobot('denso_left') if left_robot is None: raise Exception('Could not find robot: denso_left') try: left_manip = left_robot.SetActiveManipulator('denso_ft_sensor_gripper') except: raise Exception('In left_robot could not find manipulator: denso_ft_sensor_gripper') right_robot = env.GetRobot('denso_right') if right_robot is None: raise Exception('Could not find robot: denso_right') try: right_manip = right_robot.SetActiveManipulator('denso_ft_sensor_gripper') except: raise Exception('In denso_right could not find manipulator: denso_ft_sensor_gripper') # Remove unnecessary objects from the world criros.raveutils.remove_bodies(env, remove=['ikea_stick', 'pin', 'reference_frame']) logger.loginfo('Loading motion planning and control pipeline') # Configure motion planning for the left robot left_motion = RoboMotion(left_manip) left_motion.change_active_manipulator('denso_ft_sensor_gripper', iktype=iktype5D) left_motion.scale_velocity_limits(0.2) left_motion.scale_acceleration_limits(0.2) left_motion.set_smoother_iterations(100) left_motion.set_planning_iterations(50) left_motion.enable_collision_checking(True) left_motion.update_link_stats() # Configure motion planning for the right robot right_motion = RoboMotion(right_manip) right_motion.change_active_manipulator('denso_ft_sensor_gripper', iktype=iktype5D) right_motion.scale_velocity_limits(0.2) right_motion.scale_acceleration_limits(0.2) right_motion.set_smoother_iterations(100) right_motion.set_planning_iterations(50) right_motion.enable_collision_checking(True) right_motion.update_link_stats() # Add extra protection to avoid hitting the table table_aabb = env.GetKinBody('table').GetLink('base').ComputeAABB() boxdef = np.zeros(6) boxdef[:3] = table_aabb.pos() + np.array([0, 0, table_aabb.extents()[2]+0.05]) boxdef[3:5] = table_aabb.extents()[:2] + 0.05 boxdef[5] = 0.01 boxdef.shape = (1,6) with env: paranoia = orpy.RaveCreateKinBody(env, '') paranoia.SetName('paranoia_cover') paranoia.InitFromBoxes(boxdef, True) env.Add(paranoia) # OpenRAVE viewer env.SetViewer('QtCoin') # Trajectory controllers left_controller = JointTrajectoryController(namespace='left', log_level=rospy.ERROR) right_controller = JointTrajectoryController(namespace='right', log_level=rospy.ERROR) # Gripper controllers action_server = '/left/gripper/gripper_action_controller' left_gripper = actionlib.SimpleActionClient(action_server, CModelCommandAction) logger.loginfo( 'Waiting for [{0}] action server'.format(action_server) ) if not left_gripper.wait_for_server(timeout=rospy.Duration(3.0)): logger.logerr('Timed out waiting for Gripper Controller' 'Action Server to connect. Start the action server' 'before running this node.') return else: logger.loginfo( 'Successfully connected to {0}'.format(action_server) ) action_server = '/right/gripper/gripper_action_controller' right_gripper = actionlib.SimpleActionClient(action_server, CModelCommandAction) logger.loginfo( 'Waiting for [{0}] action server'.format(action_server) ) if not right_gripper.wait_for_server(timeout=rospy.Duration(3.0)): logger.logerr('Timed out waiting for Gripper Controller' 'Action Server to connect. Start the action server' 'before running this node.') return else: logger.loginfo( 'Successfully connected to {0}'.format(action_server) ) # Synch the OpenRAVE environment with TF. Give 2 secs for proper synch envid = orpy.RaveGetEnvironmentId(env) state_updater = criros.raveutils.RaveStateUpdater(envid=envid, fixed_frame='left/base_link', logger=rospy) rospy.sleep(2.0) state_updater.stop() print left_motion.robot.GetTransform() print right_motion.robot.GetTransform() # Find a pose in the middle of the two robots left_bb_center = left_motion.robot.ComputeAABB().pos() right_bb_center = right_motion.robot.ComputeAABB().pos() midpoint = (left_bb_center + right_bb_center) / 2. ldir = tr.unit_vector(right_bb_center - left_bb_center) rdir = tr.unit_vector(left_bb_center - right_bb_center) standoff = 0.0325 left_ray = orpy.Ray(midpoint-standoff*ldir, ldir) right_ray = orpy.Ray(midpoint-standoff*rdir, rdir) # Look at the middle of the robots T = tr.euler_matrix(-2, 0, np.pi/2.) T[:3,3] = [2.4, midpoint[1], 1.57] env.GetViewer().SetCamera(T) # Show the goal poses left_motion.draw_axes(criros.conversions.from_ray(left_ray)) left_motion.draw_axes(criros.conversions.from_ray(right_ray)) # Close the grippers left_motion.taskmanip.CloseFingers() right_motion.taskmanip.CloseFingers() open_cmd = CModelCommandGoal(position=0.085, velocity=0.1, force=100) close_cmd = CModelCommandGoal(position=0.0, velocity=0.1, force=100) left_gripper.send_goal(close_cmd) right_gripper.send_goal_and_wait(close_cmd) # Move left arm qinit = left_controller.get_joint_positions() qleft = left_motion.find_closest_iksolution(left_ray) traj = left_motion.generate_trajectory(qinit, qleft) ros_traj = left_motion.openrave_to_ros_traj(traj, rate=125.) left_controller.set_trajectory(ros_traj) left_controller.start() left_motion.robot.GetController().SetPath(traj) left_controller.wait() # Move right arm qinit = right_controller.get_joint_positions() qright = right_motion.find_closest_iksolution(right_ray) traj = right_motion.generate_trajectory(qinit, qright) ros_traj = right_motion.openrave_to_ros_traj(traj, rate=125.) right_controller.set_trajectory(ros_traj) right_controller.start() right_motion.robot.GetController().SetPath(traj) right_controller.wait() import IPython IPython.embed()