def move_to_pose(self, x, y, theta):
# This is a hack to get the right trajectory to pick up the can at the kitchen counter in the kitchen scenario.
if x == 0.16 and y == -0.16:
self.execCommand("move_to_pose", ["3.5", "1.5", str(theta)])
self.execCommand("move_to_pose", ["-0.45", "1.3", str(theta)])
self.execCommand("move_to_pose", ["-0.7", "-0.18", str(theta)])
self.execCommand("move_to_pose", ["-0.2", "-0.16", str(theta)])
self.execCommand("move_to_pose", ["0.12", "-0.16", str(theta)])
# This is a hack to get the right trajectory to release the can at the dining table in the kitchen scenario.
if x == 1.7 and y == 5.8:
self.execCommand("move_to_pose", ["0.23", "0.6", str(theta)])
self.execCommand("move_to_pose", ["0.5", "0.8", str(theta)])
self.execCommand("move_to_pose", ["1.2", "0.9", str(theta)])
self.execCommand("move_to_pose", ["3.5", "1.2", str(theta)])
self.execCommand("move_to_pose", ["3.5", "5.8", str(theta)])
destination = Pose()
quaternion = RobotInterface.quaternion2DFromAngle(theta)
destination.position.x = x
destination.position.y = y
destination.position.z = 0
destination.orientation.x = quaternion[0]
destination.orientation.y = quaternion[1]
destination.orientation.z = quaternion[2]
destination.orientation.w = quaternion[3]
self.moveTo3DPose(destination)
def grasp_object(self, label):
offset = RobotInterface.define_pose(-0.02, 0.06, -0.12,
# offset = RobotInterface.define_pose(1, 0.06, 1.12,
0, 0, 0.0, 0.0)
self.add_object_link('pr2::r_gripper_r_finger_tip_link', label, offset)
self.enforce_object_links(link_update_frequency=0.0)
self.grasp()
self.enforce_object_links(link_update_frequency=0.0)
def move_to_pose(self, x, y, theta):
destination = Pose()
quaternion = RobotInterface.quaternion2DFromAngle(theta)
destination.position.x = x
destination.position.y = y
destination.position.z = 0
destination.orientation.x = quaternion[0]
destination.orientation.y = quaternion[1]
destination.orientation.z = quaternion[2]
destination.orientation.w = quaternion[3]
self.walkTo3DPose(destination)
def set_angles_slow(self,stop_angles,delay=2):
start_angles=self.get_angles()
start=time.time()
stop=start+delay
r=rospy.Rate(100)
while not rospy.is_shutdown():
t=time.time()
if t>stop: break
ratio=(t-start)/delay
angles=RobotInterface.interpolate(stop_angles,start_angles,ratio)
self.set_angles(angles)
r.sleep()
def grasp_object(self, label):
self.open_gripper()
x = 0.06
y = -0.001
z = -0.1
w = 1
offset = RobotInterface.define_pose(x, y, z, 0, 0, 0.0, w)
self.lower_arms()
self.get_on_ground()
rospy.sleep(4)
self.add_object_link('darwin::MP_ARM_GRIPPER_FIX_R', label, offset)
self.enforce_object_links(link_update_frequency=0.0)
self.grasp()
self.enforce_object_links(link_update_frequency=0.0)
self.stand_up()
rospy.sleep(5)
def walkTo3DPose(self, destination):
rospy.loginfo("Staggering to given destination {}".format(str(destination)))
while not self.pose:
print "Pose has not yet been initialized, can not move."
rospy.sleep(2)
destinationPos2d = np.array([destination.position.x,destination.position.y])
euler = RobotInterface.eulerFromQuarternion(self.pose)
heading = euler[2]
locationPos2d = [self.pose.position.x,self.pose.position.y]
destAng = RobotInterface.angle_between2d(destinationPos2d,locationPos2d)
turnAng = destAng - heading
if turnAng > math.pi:
turnAng -= (2*math.pi)
if turnAng < -math.pi:
turnAng += (2*math.pi)
angThreshold = 0.15
distThreshold = 0.4
maxTurnV = 0.2
minTurnV = 0.00
maxFwdV = 1
minFwdV = 0
while True:
heading = RobotInterface.eulerFromQuarternion(self.pose)[2]
locationPos2d = [self.pose.position.x,self.pose.position.y]
destAng = RobotInterface.angle_between2d(destinationPos2d,locationPos2d)
# Determine turning speed
turnAng = destAng - heading
if turnAng > math.pi:
turnAng -= (2*math.pi)
if turnAng < -math.pi:
turnAng += (2*math.pi)
angAbs = abs(turnAng)
if turnAng < 0:
turnDir = -1
else:
turnDir = 1
turnV = angAbs / math.pi * 3
turnV = max(min(math.pow(turnV, 2), maxTurnV), minTurnV)
angV = turnV * turnDir
# Determine forward speed
distAbs = abs(np.linalg.norm(destinationPos2d - locationPos2d))
fwdV = min(math.pow(distAbs, 1.5), 1)
fwdV *= (1 - turnV/maxTurnV)
fwdV = max(min(fwdV, maxFwdV), minFwdV)
if (abs(distAbs) < distThreshold):
fwdV = 0
self.set_walk_velocity(fwdV, 0, angV)
if (abs(turnAng) < angThreshold and abs(distAbs) < distThreshold):
self.set_walk_velocity(0, 0, 0)
rospy.loginfo("Arrived at destination")
break
rospy.sleep(0.2)
def moveTo3DPose(self, destination):
rospy.loginfo("Staggering to given destination {}".format(str(destination)))
while not self.pose:
print "Pose has not yet been initialized, can not move."
rospy.sleep(2)
locationPos = np.array([self.pose.position.x,self.pose.position.y,self.pose.position.z])
destinationPos = np.array([destination.position.x,destination.position.y,destination.position.z])
delta = destinationPos - locationPos
# print delta
destinationPos2d = np.array([destination.position.x,destination.position.y])
euler = RobotInterface.eulerFromQuarternion(self.pose)
heading = euler[2]
# print " My heading is " + str(heading/math.pi*180) + " deg."
locationPos2d = [self.pose.position.x,self.pose.position.y]
# destAng = angle_between(locationPos2d,destinationPos2d)
destAng = RobotInterface.angle_between2d(destinationPos2d,locationPos2d)
# print " My destination is at " + str(destAng/math.pi*180) + " deg."
turnAng = destAng - heading
if turnAng > math.pi:
turnAng -= (2*math.pi)
if turnAng < -math.pi:
turnAng += (2*math.pi)
# print " I have to turn by " + str(turnAng/math.pi*180) + " deg."
angThreshold = 0.03
distThreshold = 0.2
while True:
heading = RobotInterface.eulerFromQuarternion(self.pose)[2]
# # locationPos2d = np.array([self.pose.position.x,self.pose.position.y])
locationPos2d = [self.pose.position.x,self.pose.position.y]
destAng = RobotInterface.angle_between2d(destinationPos2d,locationPos2d)
turnAng = destAng - heading
if turnAng > math.pi:
turnAng -= (2*math.pi)
if turnAng < -math.pi:
turnAng += (2*math.pi)
angAbs = abs(turnAng)
if turnAng < 0 :
turnDir = -1
else:
turnDir = 1
turnV = angAbs / math.pi * 15
maxTurnV = 1
minTurnV = 0.0
turnV = max(min(math.pow(turnV, 2), maxTurnV), minTurnV)
# turnV = min(angAbs,0.5)
angV = turnV * turnDir
distAbs = abs(np.linalg.norm(destinationPos2d - locationPos2d))
fwdV = min(math.pow(distAbs,1.0), 1.5)
if angAbs > (math.pi/8):
fwdV = 0
else:
fwdV *= (1-(angAbs/(math.pi/2)))
if (abs(turnAng) < angThreshold):
angV = 0
if (abs(distAbs) < distThreshold):
fwdV = 0
# print "heading: " + str(heading) + " -- destAng: " + str(destAng) + " -- turnAng: " + str(turnAng) + " -- angV: " + str (angV) + " -- dist: " + str(distAbs) + " -- fwdV: " + str(fwdV)
# break
self.set_move_velocity(fwdV,0,angV)
if (abs(turnAng) < angThreshold and abs(distAbs) < distThreshold):
self.set_move_velocity(0,0,0)
rospy.loginfo("Arrived at destination")
break
rospy.sleep(0.2)
