def create_affine_trajectory(robot, poses):
''' Creates an untimed openrave affine trajectory from a set of affine poses '''
doft = openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis
cspec = openravepy.RaveGetAffineConfigurationSpecification(doft, robot)
traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), 'GenericTrajectory')
traj.Init(cspec)
for iwaypoint, pose in enumerate(poses):
waypoint = openravepy.RaveGetAffineDOFValuesFromTransform(pose, doft)
traj.Insert(iwaypoint, waypoint)
return traj
def executePath(robot, path, resolution, handles):
print 'path: ' + str(len(path))
dis_poses = discretizePath(path, resolution)
print 'dis_poses: ' + str(len(dis_poses))
poses = []
base_poses = []
all_poses = []
all_poses.append(dis_poses)
base_goal = []
arm_goal = []
for i in range(len(dis_poses) - 1):
pose, base_pose, finalgoal = executeVelPath(robot, dis_poses[i + 1],
handles)
arm_goal.append(finalgoal[:8])
base_goal.append(finalgoal[-3:])
poses.append(pose)
base_poses.append(base_pose)
all_poses.append(poses)
all_poses.append(base_poses)
print 'size of base points: ' + str(len(base_goal))
print 'size of arm points: ' + str(len(arm_goal))
from prpy.rave import save_trajectory
#Creating base Traj
doft = openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis
cspec = openravepy.RaveGetAffineConfigurationSpecification(doft, robot)
base_traj = openravepy.RaveCreateTrajectory(robot.GetEnv(),
'GenericTrajectory')
base_traj.Init(cspec)
for i in range(len(base_goal)):
base_traj.Insert(i, base_goal[i])
save_trajectory(base_traj,
'/home/abhi/Desktop/traj2/base_untimed_traj.xml')
print 'base_traj saved'
#Creating Arm Traj
jointnames = [
'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint',
'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint'
]
robot.SetActiveDOFs(
[robot.GetJoint(name).GetDOFIndex() for name in jointnames], )
acspec = robot.GetActiveConfigurationSpecification('linear')
arm_traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), '')
arm_traj.Init(acspec)
for i in range(len(arm_goal)):
arm_traj.Insert(i, arm_goal[i])
save_trajectory(arm_traj, '/home/abhi/Desktop/traj2/arm_untimed_traj.xml')
print 'arm_traj saved'
return all_poses
def ConvertPlanToTrajectory(self, plan):
# Create a trajectory and insert all points
traj = openravepy.RaveCreateTrajectory(self.robot.GetEnv(), 'GenericTrajectory')
doft = openravepy.DOFAffine.X | openravepy.DOFAffine.Y
config_spec = openravepy.RaveGetAffineConfigurationSpecification(doft, self.robot)
traj.Init(config_spec)
idx = 0
for pt in plan:
traj.Insert(idx, pt)
idx = idx + 1
return traj
def ExtractBTrajFromWholeBodyTraj(robot, traj):
doft = openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis
cspec = traj.GetConfigurationSpecification()
bcspec = openravepy.RaveGetAffineConfigurationSpecification(doft, robot)
base_traj = openravepy.RaveCreateTrajectory(robot.GetEnv(), 'GenericTrajectory')
bcspec.AddGroup('affine_velocities', dof= 3, interpolation='next')
bcspec.AddDeltaTimeGroup()
base_traj.Init(bcspec)
for iwaypoint in xrange(traj.GetNumWaypoints()):
value = []
waypoint = traj.GetWaypoint(iwaypoint)
value.extend(waypoint[8:11])
value.extend(waypoint[19:23])
base_traj.Insert(iwaypoint, value)
return base_traj
def PlanToBasePose(self,
robot,
goal_pose,
timelimit=60.0,
return_first=False,
**kw_args):
"""
Plan to a base pose using SBPL
@param robot
@param goal_pose desired base pose
@param timelimit timeout in seconds
@param return_first return the first path found (if true, the planner will run until a path is found, ignoring the time limit)
"""
params = openravepy.Planner.PlannerParameters()
from openravepy import DOFAffine
robot.SetActiveDOFs([],
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis)
params.SetRobotActiveJoints(robot)
config_spec = openravepy.RaveGetAffineConfigurationSpecification(
DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, robot)
#params.SetConfigurationSpecification(self.env, config_spec) # This breaks
goal_config = openravepy.RaveGetAffineDOFValuesFromTransform(
goal_pose, DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis)
params.SetGoalConfig(goal_config)
# Setup default extra parameters
extra_params = self.planner_params
limits = robot.GetAffineTranslationLimits()
extents = [limits[0][0], limits[1][0], limits[0][1], limits[1][1]]
extra_params["extents"] = extents
extra_params["timelimit"] = timelimit
if return_first:
extra_params["return_first"] = 1
else:
extra_params["return_first"] = 0
extra_params["initial_eps"] = 1.0
for key, value in kw_args.iteritems():
extra_params[key] = value
params.SetExtraParameters(str(extra_params))
traj = openravepy.RaveCreateTrajectory(self.env, '')
try:
self.planner.InitPlan(robot, params)
status = self.planner.PlanPath(traj, releasegil=True)
except Exception as e:
raise PlanningError('Planning failed with error: {0:s}'.format(e))
from openravepy import PlannerStatus
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError('Planner returned with status {0:s}.'.format(
str(status)))
return traj