def performNavigationPlanning(self, goal, execute=True, draw_marker=False, steplength=0.1):
goal = np.array(goal)
if goal.ndim == 2:
# assume a 4x4 transformation matrix
angle = openravepy.axisAngleFromRotationMatrix(goal)[-1]
goal = np.array([goal[0, -1], goal[1, -1], angle])
# find the boundaries of the environment
envmin, envmax = utils.get_environment_limits(self.env, self.robot)
with self.env:
self.robot.SetAffineTranslationLimits(envmin, envmax)
self.robot.SetAffineTranslationMaxVels([0.3, 0.3, 0.3])
self.robot.SetAffineRotationAxisMaxVels(np.ones(4))
self.robot.SetActiveDOFs(
[], openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis, [0, 0, 1]
)
# draw the marker
if draw_marker:
center = np.r_[goal[0:2], 0.2]
xaxis = 0.5 * np.array((np.cos(goal[2]), np.sin(goal[2]), 0))
yaxis = 0.25 * np.array((-np.sin(goal[2]), np.cos(goal[2]), 0))
h = self.env.drawlinelist(
np.transpose(
np.c_[
center - xaxis,
center + xaxis,
center - yaxis,
center + yaxis,
center + xaxis,
center + 0.5 * xaxis + 0.5 * yaxis,
center + xaxis,
center + 0.5 * xaxis - 0.5 * yaxis,
]
),
linewidth=5.0,
colors=np.array((0, 1, 0)),
)
openravepy.RaveLogInfo("Planning to goal " + str(goal))
res = self.basemanip.MoveActiveJoints(
goal=goal, maxiter=3000, steplength=steplength, execute=execute, outputtrajobj=True
)
if res is None:
raise ValueError("Could not find a trajectory")
if execute:
openravepy.RaveLogInfo("Waiting for controller to finish")
self.robot.WaitForController(0)
self.robot.GetController().Reset()
return res
def performNavigationPlanning(self,
goal,
execute=True,
draw_marker=False,
steplength=0.1):
goal = np.array(goal)
if goal.ndim == 2:
#assume a 4x4 transformation matrix
angle = openravepy.axisAngleFromRotationMatrix(goal)[-1]
goal = np.array([goal[0, -1], goal[1, -1], angle])
# find the boundaries of the environment
envmin, envmax = utils.get_environment_limits(self.env, self.robot)
with self.env:
self.robot.SetAffineTranslationLimits(envmin, envmax)
self.robot.SetAffineTranslationMaxVels([0.3, 0.3, 0.3])
self.robot.SetAffineRotationAxisMaxVels(np.ones(4))
self.robot.SetActiveDOFs(
[], openravepy.DOFAffine.X | openravepy.DOFAffine.Y
| openravepy.DOFAffine.RotationAxis, [0, 0, 1])
# draw the marker
if draw_marker:
center = np.r_[goal[0:2], 0.2]
xaxis = 0.5 * np.array((np.cos(goal[2]), np.sin(goal[2]), 0))
yaxis = 0.25 * np.array((-np.sin(goal[2]), np.cos(goal[2]), 0))
h = self.env.drawlinelist(np.transpose(
np.c_[center - xaxis, center + xaxis, center - yaxis,
center + yaxis, center + xaxis,
center + 0.5 * xaxis + 0.5 * yaxis, center + xaxis,
center + 0.5 * xaxis - 0.5 * yaxis]),
linewidth=5.0,
colors=np.array((0, 1, 0)))
openravepy.RaveLogInfo("Planning to goal " + str(goal))
res = self.basemanip.MoveActiveJoints(goal=goal,
maxiter=3000,
steplength=steplength,
execute=execute,
outputtrajobj=True)
if res is None:
raise ValueError("Could not find a trajectory")
if execute:
openravepy.RaveLogInfo("Waiting for controller to finish")
self.robot.WaitForController(0)
self.robot.GetController().Reset()
return res
def generate_random_pos(robot, obj_to_grasp = None):
"""Generate a random position for the robot within the boundaries of the
world.
"""
T = robot.GetTransform()
return T
envmin, envmax = utils.get_environment_limits(robot.GetEnv(), robot)
if obj_to_grasp is None:
max_x = envmax[0]
max_y = envmax[1]
max_th = np.pi
min_x = envmin[0]
min_y = envmin[1]
min_th = np.pi
else:
if type(obj_to_grasp) is openravepy.KinBody:
obj_pos = obj_to_grasp.GetTransform()[:3,-1]
else:
obj_pos = obj_to_grasp
max_x = min(obj_pos[0] + 1.2, envmax[0])
max_y = min(obj_pos[1] + 1.2, envmax[1])
min_x = max(obj_pos[0] - 1.2, envmin[0])
min_y = max(obj_pos[1] - 1.2, envmin[1])
#print "X ", (min_x, max_x), " Y: ", (min_y, max_y)
x = np.random.uniform(min_x, max_x)
y = np.random.uniform(min_y, max_y)
z = T[2,3]
if obj_to_grasp is None:
th = np.random.uniform(min_th, max_th)
else:
robot_pos = T[:3,-1]
facing_angle = np.arctan2(obj_pos[1] -y , obj_pos[0] - x)
th = np.random.uniform(facing_angle - np.pi/4.,
facing_angle + np.pi/4.)
#rotation
T = openravepy.matrixFromAxisAngle([0,0,th])
#translation
T[:, -1] = [x, y, z, 1]
return T
