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 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 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 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()
