def stick_transition(self, prior, pose):
# sticking also proportional to angle
degrees = DegreeOfFreedom()
stick_prob = 0.0
if prior > 0:
max_angles = self.obj.max_angles(self.goal)
orien = gt.quaternion_to_euler(pose.pose.orientation)
# height factor
h = (pose.pose.position.z-(-2.8)) / 2.8
# probability of sticking is higher on low angles
# but sliding increases with high angles
# for now with only stick, min sliding since we cant account
# for it and just deal with sticking
# assumes upright = 0 and only tips forward
# normalize angle. prob of tipping is the complement
if orien[0]-max_angles[0] < 0:
x_stick = 1.0 # just a failure. but not really sticking
else:
x_stick = ((orien[0] - max_angles[0]) / (math.pi/2 - max_angles[0]))
if orien[1]-max_angles[1] < 0:
y_stick = 1.0
else:
y_stick = ((orien[1] - max_angles[1]) / (math.pi/2 - max_angles[1]))
degrees.modify(0, y_stick)
degrees.modify(1, x_stick)
stick_prob = (x_stick+h, y_stick+h)
return (stick_prob, degrees)
else:
return ((0.0, 0.0), degrees)
def execute_base_trajectory(self, trajectory):
goal = BaseTrajectoryGoal()
#be a little slower and more precise
goal.linear_velocity = 0.2
goal.angular_velocity = np.pi / 8.0
goal.linear_error = 0.01
goal.angular_error = 0.01
joint = -1
for (i, n) in enumerate(
trajectory.multi_dof_joint_trajectory.joint_names):
if n == 'world_joint' or n == '/world_joint':
goal.world_frame = trajectory.multi_dof_joint_trajectory.frame_ids[
i]
goal.robot_frame = trajectory.multi_dof_joint_trajectory.child_frame_ids[
i]
joint = i
break
if joint < 0:
raise ActionFailedError('No world joint found in base trajectory')
for p in trajectory.multi_dof_joint_trajectory.points:
(phi, theta,
psi) = gt.quaternion_to_euler(p.poses[joint].orientation)
goal.trajectory.append(
Pose2D(x=p.poses[joint].position.x,
y=p.poses[joint].position.y,
theta=psi))
self.base_action.send_goal_and_wait(goal)
def tip_transition(self, prior, pose):
'''
@type prob: float
@param prob: probability of tipping over all configurations
'''
degrees = DegreeOfFreedom()
tip_prob = 0.0
if prior > 0:
max_angles = self.obj.max_angles(self.goal)
orien = gt.quaternion_to_euler(pose.pose.orientation)
# height factor
h = ((pose.pose.position.z-(-2.8)) / 2.8)
# print "pose.z="+str(pose.pose.position.z)+" diff="+str(pose.pose.position.z+2.8)
# print "h prob="+str(h)
# assumes upright = 0 and only tips forward
# normalize angle. prob of tipping is the complement
if orien[0]-max_angles[0] < 0:
x_tip = 0.0
else:
x_tip = ((orien[0] - max_angles[0]) / (math.pi/2 - max_angles[0]))
if orien[1]-max_angles[1] < 0:
y_tip = 0.0
else:
y_tip = ((orien[1] - max_angles[1]) / (math.pi/2 - max_angles[1]))
# print "orien="+str(orien[0])+"max="+str(max_angles[0])
# print "x_tip="+str(x_tip)+"y_tip="+str(y_tip)
degrees.modify(0, x_tip)
degrees.modify(1, y_tip)
tip_prob = (x_tip+h, y_tip+h)
return (tip_prob, degrees)
else:
return ((0.0, 0.0), degrees)
def execute(self):
'''
Tip motion primitive.
describes motion of an object if it is subject to tipping.
resulting degrees of freedom will be rotational
depends on angles of release.
'''
max_angles = self.mobj.object.max_angles(self.goal)
orien = gt.quaternion_to_euler(self.pose.pose.orientation)
# orien = quat_to_orien(self.pose.pose.orientation)
#orien = Orientation(0.0, math.pi/2,0.0)
x_tip = abs(orien[0] - max_angles[0])
y_tip = abs(orien[1] - max_angles[1])
print "max_angles=" + str(max_angles)
print "x_tip=" + str(x_tip) + " y_tip=" + str(y_tip)
if (x_tip > 0) and (y_tip > 0):
return DegreeOfFreedom(
[0.0, 0.0, 0.0, math.pi / 2, math.pi / 2, 0.0])
elif x_tip > 0:
return DegreeOfFreedom([0.0, 0.0, 0.0, math.pi / 2, 0.0, 0.0])
elif y_tip > 0:
return DegreeOfFreedom([0.0, 0.0, 0.0, 0.0, math.pi / 2, 0.0])
else:
return DegreeOfFreedom([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
def real_best_placements(self, goal_pose):
placements = []
# max_angles = self.max_angles(pose)
max_angles = self.max_angles(goal_pose)
print "max angles=" + str(max_angles)
max_height = 0.15
angle = -25. * (math.pi / 180.)
h = 0.023
placements.append((h, 0.0, 0.0, angle))
placements.append((h, 0.0, 0.0, angle))
placements.append((h, 0.0, 0.0, angle))
placements.append((h, 0.0, 0.0, angle))
placements.append((h, 0.0, 0.0, angle))
if max_angles[1] < max_angles[0]:
for h in frange(0.0, max_height, 0.1):
# for phi in frange(0.0, max_angles[0], 0.1):
# for theta in frange(0.0, max_angles[1], 0.1):
for psi in frange(0.0, -max_angles[1], -0.1):
placements.append((h, 0.0, 0.0, psi))
for h in frange(0.0, max_height, 0.1):
# for phi in frange(max_angles[0], math.pi/2, 0.1):
# for theta in frange(max_angles[1], math.pi/2, 0.1):
for psi in frange(-max_angles[1], -math.pi / 2, -0.1):
placements.append((h, 0.0, 0.0, psi))
else:
for h in frange(0.0, max_height, 0.1):
# for phi in frange(0.0, max_angles[0], 0.1):
for theta in frange(0.0, max_angles[0], 0.1):
# for psi in frange(0.0, -max_angles[1], -0.1):
placements.append((h, 0.0, theta, 0.0))
for h in frange(0.0, max_height, 0.1):
# for phi in frange(max_angles[0], math.pi/2, 0.1):
for theta in frange(max_angles[0], math.pi / 2, 0.1):
# for psi in frange(-max_angles[1], -math.pi/2, -0.1):
placements.append((h, 0.0, theta, 0.0))
mode = modes.Mode(self, goal_pose)
best_releases = []
for place in placements:
best_releases.append(tools.make_pose(goal_pose, place[0], \
gt.euler_to_quaternion(\
place[1],place[2],place[3])))
best_releases = sorted(best_releases,
key=lambda pose: mode.fall_probability(pose))
# best_releases.reverse()
for r in best_releases:
# print "prob="+str(mode.fall_probability(r))
angles = gt.quaternion_to_euler(r.pose.orientation)
c = (180 / math.pi)
print "angles=" + str(angles[0] * c) + " " + str(
angles[1] * c) + " " + str(angles[2] * c)
# print "prob="+str(mode.fall_probability(r))+" "+str(r.pose.position.z+2.8)+" angles="+str((180/math.pi)*angles[0])+" "+str((180/math.pi)*angles[1])+" "+str((180/math.pi)*angles[2])
return best_releases
def execute(self):
# self.mobj = mobj
# self.goal = goal
# self.pose = pose
# self.vels = vels
# inv = Quaternion()
# inv.x = -self.pose.pose.orientation.x
# inv.y = -self.pose.pose.orientation.y
# inv.z = -self.pose.pose.orientation.z
# inv.w = -self.pose.pose.orientation.w
[phi, theta, psi] = gt.quaternion_to_euler(self.pose.pose.orientation)
dof = DegreeOfFreedom([2.0, 0.0, 0.0, 0.0, 0.0, 0.0])
return dof
def execute_warp_trajectory(self, trajectory):
#figure out the last point on the trajectory
tp = trajectory.multi_dof_joint_trajectory.points[-1]
(phi, theta, psi) = gt.quaternion_to_euler(tp.poses[0].orientation)
self.base.move_to(tp.poses[0].position.x, tp.poses[0].position.y, psi)
def max_angles(self, pose):
'''
@type pose: PoseStamped
@param pose: the pose the object will be released in (in the frame of the robot)
'''
## get the center of mass in the frame of the object,
## then transform it into the robot frame
cm = self.center_of_mass(pose).tolist() ## in the frame of the object
tf_cm = self.translate(cm, pose)
## get the corners of the base of the bounding box of the object in robot frame
base = self.get_base(pose.pose)
# print "base="+str([pivot.position.x, pivot.position.y, pivot.position.z])
print "base=" + str(base)
## corner of the base is the pivot pt, transform to obj frame
# pivot_matrix = self.inv_translate([[base[3][0]],[base[3][1]],[base[3][2]]], pose).tolist()
# pivot = [pivot_matrix[0][0], pivot_matrix[1][0], pivot_matrix[2][0]]
## get the euler angles of the pose (goal pose)
(orig_phi, orig_theta,
orig_psi) = geometry_tools.quaternion_to_euler(pose.pose.orientation)
## max rotation of pi/2
max_phi = orig_phi + math.pi / 2
max_theta = orig_theta + math.pi / 2
pivot = Point()
pivot.x = base[0][0]
pivot.y = base[0][1]
pivot.z = base[0][2]
cm = Pose()
cm.position.x = tf_cm[0][0]
cm.position.y = tf_cm[1][0]
cm.position.z = tf_cm[2][0]
cm.orientation = pose.pose.orientation
t = geometry_tools.inverse_transform_point(pivot, cm)
tol = 0.01
## find max angle around x-axis, phi
print "cm=" + str(tf_cm.tolist())
for phi in frange(0.0, math.pi / 2, 0.1):
transform = Pose()
transform.orientation = geometry_tools.euler_to_quaternion(
phi, 0.0, 0.0)
cm_new = Pose()
cm_new.position = cm.position
cm_new.orientation = geometry_tools.transform_quaternion(
cm.orientation, transform)
p_new = geometry_tools.transform_point(t, cm_new)
y_diff = abs(tf_cm[1][0] - p_new.y)
z_diff = abs(tf_cm[2][0] - p_new.z)
# print "phi angle="+str(phi)+" pivot="+str([p_new.x, p_new.y, p_new.z])
if y_diff < tol or z_diff < tol:
max_phi = phi
break
for theta in frange(0.0, math.pi / 2, 0.1):
transform = Pose()
transform.orientation = geometry_tools.euler_to_quaternion(
0.0, theta, 0.0)
cm_new = Pose()
cm_new.position = cm.position
cm_new.orientation = geometry_tools.transform_quaternion(
cm.orientation, transform)
p_new = geometry_tools.transform_point(t, cm_new)
x_diff = abs(tf_cm[0][0] - p_new.x)
z_diff = abs(tf_cm[2][0] - p_new.z)
# print "theta angle="+str(theta)+" pivot="+str([p_new.x, p_new.y, p_new.z])
if z_diff < tol or x_diff < tol:
max_theta = theta
break
return (max_phi, max_theta)