def PlanToEndEffectorPose(self, robot, goal_pose, timelimit=5.0,
**kw_args):
"""
Plan to an end effector pose by first creating a geodesic
trajectory in SE(3) from the starting end-effector pose to the goal
end-effector pose, and then attempting to follow it exactly
using PlanWorkspacePath.
@param robot
@param goal_pose desired end-effector pose
@return traj
"""
with robot:
# Create geodesic trajectory in SE(3)
manip = robot.GetActiveManipulator()
start_pose = manip.GetEndEffectorTransform()
traj = openravepy.RaveCreateTrajectory(self.env, '')
spec = openravepy.IkParameterization.\
GetConfigurationSpecificationFromType(
openravepy.IkParameterizationType.Transform6D,
'linear')
traj.Init(spec)
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(start_pose))
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(goal_pose))
openravepy.planningutils.RetimeAffineTrajectory(
traj,
maxvelocities=0.1*numpy.ones(7),
maxaccelerations=0.1*numpy.ones(7)
)
return self.PlanWorkspacePath(robot, traj, timelimit)
def PlanToEndEffectorOffset(self,
robot,
direction,
distance,
max_distance=None,
timelimit=5.0,
**kw_args):
"""
Plan to a desired end-effector offset with move-hand-straight
constraint. movement less than distance will return failure.
The motion will not move further than max_distance.
@param robot
@param direction unit vector in the direction of motion
@param distance minimum distance in meters
@param max_distance maximum distance in meters
@param timelimit timeout in seconds
@return traj
"""
if distance < 0:
raise ValueError('Distance must be non-negative.')
elif numpy.linalg.norm(direction) == 0:
raise ValueError('Direction must be non-zero')
elif max_distance is not None and max_distance < distance:
raise ValueError('Max distance is less than minimum distance.')
elif max_distance is not None and not numpy.isfinite(max_distance):
raise ValueError('Max distance must be finite.')
# Normalize the direction vector.
direction = numpy.array(direction, dtype='float')
direction /= numpy.linalg.norm(direction)
with robot:
manip = robot.GetActiveManipulator()
start_pose = manip.GetEndEffectorTransform()
traj = openravepy.RaveCreateTrajectory(self.env, '')
spec = openravepy.IkParameterization.\
GetConfigurationSpecificationFromType(
openravepy.IkParameterizationType.Transform6D, 'linear')
traj.Init(spec)
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(start_pose))
min_pose = numpy.copy(start_pose)
min_pose[0:3, 3] += distance * direction
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(min_pose))
if max_distance is not None:
max_pose = numpy.copy(start_pose)
max_pose[0:3, 3] += max_distance * direction
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(max_pose))
openravepy.planningutils.RetimeAffineTrajectory(
traj,
maxvelocities=0.1 * numpy.ones(7),
maxaccelerations=0.1 * numpy.ones(7))
return self.PlanWorkspacePath(robot,
traj,
timelimit,
min_waypoint_index=1)
def PlanToEndEffectorPose(self,
robot,
goal_pose,
timelimit=5.0,
**kw_args):
"""
Plan to an end effector pose by first creating a geodesic
trajectory in SE(3) from the starting end-effector pose to the goal
end-effector pose, and then attempting to follow it exactly
using PlanWorkspacePath.
@param robot
@param goal_pose desired end-effector pose
@return traj
"""
with robot:
# Create geodesic trajectory in SE(3)
manip = robot.GetActiveManipulator()
start_pose = manip.GetEndEffectorTransform()
traj = openravepy.RaveCreateTrajectory(self.env, '')
spec = openravepy.IkParameterization.\
GetConfigurationSpecificationFromType(
openravepy.IkParameterizationType.Transform6D, 'linear')
traj.Init(spec)
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(start_pose))
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(goal_pose))
openravepy.planningutils.RetimeAffineTrajectory(
traj,
maxvelocities=0.1 * numpy.ones(7),
maxaccelerations=0.1 * numpy.ones(7))
return self.PlanWorkspacePath(robot, traj, timelimit)
def PlanToEndEffectorOffset(self, robot, direction, distance,
max_distance=None, timelimit=5.0,
**kw_args):
"""
Plan to a desired end-effector offset with move-hand-straight
constraint. movement less than distance will return failure.
The motion will not move further than max_distance.
@param robot
@param direction unit vector in the direction of motion
@param distance minimum distance in meters
@param max_distance maximum distance in meters
@param timelimit timeout in seconds
@return traj
"""
if distance < 0:
raise ValueError('Distance must be non-negative.')
elif numpy.linalg.norm(direction) == 0:
raise ValueError('Direction must be non-zero')
elif max_distance is not None and max_distance < distance:
raise ValueError('Max distance is less than minimum distance.')
elif max_distance is not None and not numpy.isfinite(max_distance):
raise ValueError('Max distance must be finite.')
# Normalize the direction vector.
direction = numpy.array(direction, dtype='float')
direction /= numpy.linalg.norm(direction)
with robot:
manip = robot.GetActiveManipulator()
start_pose = manip.GetEndEffectorTransform()
traj = openravepy.RaveCreateTrajectory(self.env, '')
spec = openravepy.IkParameterization.\
GetConfigurationSpecificationFromType(
openravepy.IkParameterizationType.Transform6D, 'linear')
traj.Init(spec)
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(start_pose))
min_pose = numpy.copy(start_pose)
min_pose[0:3, 3] += distance * direction
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(min_pose))
if max_distance is not None:
max_pose = numpy.copy(start_pose)
max_pose[0:3, 3] += max_distance * direction
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(max_pose))
openravepy.planningutils.RetimeAffineTrajectory(
traj,
maxvelocities=0.1 * numpy.ones(7),
maxaccelerations=0.1 * numpy.ones(7)
)
return self.PlanWorkspacePath(robot, traj,
timelimit, min_waypoint_index=1)
def PlanToEndEffectorOffset(self, robot, direction,
distance,
threshold,
matching_fn_class,
distance_metric_fn,
ignore_segments=True,
max_recursion_depth=10,
terminate_on_max_recursion=True,
**kwargs):
"""
Add docstring
"""
print "in RecursiveSnap: PlanToEndEffectorOffset() \n"
if distance <= 0.0:
raise ValueError("Distance must be non-negative.")
elif numpy.linalg.norm(direction) == 0:
raise ValueError("Direction must be non-zero")
# Normalize the direction vector.
direction = numpy.array(direction, dtype='float')
direction /= numpy.linalg.norm(direction)
with robot:
manip = robot.GetActiveManipulator()
start_pose = manip.GetEndEffectorTransform()
traj = openravepy.RaveCreateTrajectory(self.env, '')
spec = openravepy.IkParameterization.\
GetConfigurationSpecificationFromType(
openravepy.IkParameterizationType.Transform6D,
'linear')
traj.Init(spec)
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(start_pose))
min_pose = numpy.copy(start_pose)
min_pose[0:3, 3] += distance*direction
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(min_pose))
return self.PlanWorkspacePath(robot, traj,
threshold,
matching_fn_class,
distance_metric_fn,
ignore_segments,
max_recursion_depth,
terminate_on_max_recursion,
**kwargs
)
def PlanToEndEffectorPose(self,
robot,
goal_pose,
timelimit=5.0,
**kw_args):
"""
Plan to an end effector pose by first creating a geodesic
trajectory in SE(3) from the starting end-effector pose to the goal
end-effector pose, and then attempting to follow it exactly
using PlanWorkspacePath.
@param robot
@param goal_pose desired end-effector pose
@return traj
"""
with robot:
# Create geodesic trajectory in SE(3)
env = robot.GetEnv()
manip = robot.GetActiveManipulator()
start_pose = manip.GetEndEffectorTransform()
traj = openravepy.RaveCreateTrajectory(env, '')
spec = openravepy.IkParameterization.\
GetConfigurationSpecificationFromType(
openravepy.IkParameterizationType.Transform6D, 'linear')
traj.Init(spec)
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(start_pose))
traj.Insert(traj.GetNumWaypoints(),
openravepy.poseFromMatrix(goal_pose))
with robot.CreateRobotStateSaver(
Robot.SaveParameters.LinkTransformation):
openravepy.planningutils.RetimeAffineTrajectory(
traj,
maxvelocities=0.1 * numpy.ones(7),
maxaccelerations=0.1 * numpy.ones(7))
qtraj = self.PlanWorkspacePath(robot, traj, timelimit)
# modify tags to reflect that we won't care about
# the entire path, but only the final pose
SetTrajectoryTags(qtraj, {
Tags.CONSTRAINED: False,
Tags.SMOOTH: True
},
append=True)
return qtraj
def sequence_goals(self):
if not len(self.goal_array):
self.goal = [None]
return
goals = numpy.array(deepcopy(self.goal_array))
tasks_msg = PoseArray()
for goal in goals:
T = openravepy.transformLookat(
goal + [0.1, 0.0, 0.0],
goal - [self.starting_position_offset, 0.0, 0.0], [0, 0, -1])
T = numpy.dot(T, numpy.linalg.inv(self.T_G2EE))
pose = openravepy.poseFromMatrix(T)
pose_msg = Pose()
pose_msg.orientation.w = pose[0]
pose_msg.orientation.x = pose[1]
pose_msg.orientation.y = pose[2]
pose_msg.orientation.z = pose[3]
pose_msg.position.x = pose[4]
pose_msg.position.y = pose[5]
pose_msg.position.z = pose[6]
tasks_msg.poses.append(pose_msg)
resp = self.sequencer_client.call(tasks_msg, SEQUENCING_TYPE)
self.sequenced_goals = [goals[i] for i in resp.sequence]
self.sequenced_trajectories = resp.database_trajectories
self.num_goals_history = len(self.sequenced_goals)
print("sequenced goals: " + str(self.sequenced_goals))
print("resp.sequence: " + str(resp.sequence))
if self.sim:
self.goal = numpy.array(self.sequenced_goals[0])
self.goal_off = self.goal - self.go_to_goal_offset * self.normalize(
self.goal - self.ee_position)
self.starting_position = self.goal - numpy.array(
[self.starting_position_offset, 0.0, 0.0])
self.starting_direction = numpy.array([1.0, 0.0, 0.0])
def get_quat_from_matrix(transform):
"""
OpenRAVE Convention: Quaternions are defined with the scalar value as the first component. For example [w x y z]
"""
quat = openravepy.poseFromMatrix(transform)[0:4]
quat.tolist().reverse()
return tuple(quat)
def joints2xyzquat(manip, link_name, joint_vals):
robot, T = manip.GetRobot(), None
with robot:
robot.SetDOFValues(joint_vals, manip.GetArmIndices())
T = robot.GetLink(link_name).GetTransform()
quat_xyz = rave.poseFromMatrix(T)
return quat_xyz[4:7], quat_xyz[:4] # xyz, quat
def joints2xyzquat(manip, link_name, joint_vals):
robot, T = manip.GetRobot(), None
with robot:
robot.SetDOFValues(joint_vals, manip.GetArmIndices())
T = robot.GetLink(link_name).GetTransform()
quat_xyz = rave.poseFromMatrix(T)
return quat_xyz[4:7], quat_xyz[:4] # xyz, quat
def CreateWorkspaceTrajectoryByLinearInterpBetweenConfigs(env, robot,
q0, q1):
"""
Create a workspace trajectory by linearly interpolating
between two joint configurations.
"""
# Get a jointspace trajectory between the two configurations
# using linear joint interpolation
path = None
samples = None
with robot: # save robot state
_,samples = InterpBetweenConfigs(env, robot, q0, q1)
# restore robot state
traj = openravepy.RaveCreateTrajectory(env, '')
velocity_interp = 'linear'
spec = openravepy.IkParameterization.\
GetConfigurationSpecificationFromType(
openravepy.IkParameterizationType.Transform6D,
velocity_interp)
traj.Init(spec)
# Create a workspace path of end-effector poses.
# Each joint configuration 'q' sampled is L2 norm
# from the previous configuration.
for idx,(t,q) in enumerate(samples):
T_current = prpy.util.GetForwardKinematics(robot, q)
traj.Insert(idx, openravepy.poseFromMatrix(T_current))
# If q0 and q1 are close together, only q0 will exist in 'samples'
# and the workspace trajectory will only have one waypoint,
# therefore we manually add q1 as a second waypoint.
num_waypoints = traj.GetNumWaypoints()
if num_waypoints == 1:
T_goal = prpy.util.GetForwardKinematics(robot, q1)
traj.Insert(num_waypoints, openravepy.poseFromMatrix(T_goal))
# Add timing so we can continuously sample the trajectory
openravepy.planningutils.RetimeAffineTrajectory(traj, maxvelocities=0.1*numpy.ones(7), maxaccelerations=0.1*numpy.ones(7))
return traj
def callback(request):
for t in pose_constraints:
pose = orpy.poseFromMatrix(pose_constraints[t])
quat, xyz = pose[:4], pose[4:]
request['constraints'].append({
'type': 'pose',
'params': {
'timestep': t,
'xyz': xyz.tolist(),
'wxyz': quat.tolist(),
'link': 'j2s7s300_link_7'
}
})
def get_body_markers(self):
""" Returns a list of visualization_msgs/MarkerArray with all the links of each body in the environment """
body_markers = []
# Get all the bodies in the OpenRAVE environment
bodies = self.env.GetBodies()
for body in bodies:
print("Found body with name: {}".format(body.GetName()))
body_marker = MarkerArray()
# Choose a random color for this body
color_r = random.random()
color_g = random.random()
color_b = random.random()
# Create a separate marker for each link
for link in body.GetLinks():
print(" Link name: {}".format(link.GetName()))
link_transform = link.GetTransform()
link_marker = Marker()
link_marker.header.frame_id = '/root'
link_marker.header.stamp = rospy.get_rostime()
link_marker.ns = body.GetName() + '/link/' + link.GetName()
link_marker.id = 0
link_marker.type = Marker.SPHERE
pose = openravepy.poseFromMatrix(link_transform)
link_marker.pose.position.x = pose[4]
link_marker.pose.position.y = pose[5]
link_marker.pose.position.z = pose[6]
link_marker.pose.orientation.x = pose[1]
link_marker.pose.orientation.y = pose[2]
link_marker.pose.orientation.z = pose[3]
link_marker.pose.orientation.w = pose[0]
link_marker.scale.x = 0.2
link_marker.scale.y = 0.1
link_marker.scale.z = 0.1
link_marker.color.r = color_r
link_marker.color.g = color_g
link_marker.color.b = color_b
link_marker.color.a = 0.50
link_marker.lifetime = rospy.Duration(0)
body_marker.markers.append(link_marker)
body_markers.append(body_marker)
return body_markers
def get_robot_ee_position(self, msg):
# ee_orientation = pyquaternion.Quaternion(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z)
# ee_position = numpy.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
pose = [
msg.pose.orientation.w, msg.pose.orientation.x,
msg.pose.orientation.y, msg.pose.orientation.z,
msg.pose.position.x, msg.pose.position.y, msg.pose.position.z
]
pose = openravepy.matrixFromPose(pose)
ee_pose = numpy.dot(pose, self.T_G2EE)
ee_pose = openravepy.poseFromMatrix(ee_pose)
self.ee_orientation = pyquaternion.Quaternion(ee_pose[:4])
self.ee_position = numpy.array(ee_pose[4:])
def create_cylinder(env, table, radius, height, body_name, max_radial_distance, rand, color):
robot_pos = openravepy.poseFromMatrix(env.GetRobots()[0].GetTransform())[4:7]
min_x, max_x, min_y, max_y, table_height = utils.get_object_limits(table)
x = rand.uniform(min_x + radius, max_x - radius)
y = rand.uniform(min_y + radius, max_y - radius)
# while (x - robot_pos[0])**2 + (y - robot_pos[1])**2 > max_radial_distance:
# x = rand.uniform(min_x + radius, max_x - radius)
# y = rand.uniform(min_y + radius, max_y - radius)
z = table_height + height / 2
t = openravepy.matrixFromPose([1, 0, 0, 0, x, y, z])
cylinder = object_models.create_cylinder(env, body_name, t, [radius, height], color)
env.Add(cylinder, False)
return cylinder
def FindIKSolution(self, T, qref=None, checkcollision=True):
self.ActivateIKSolver()
# Use IKFast
with self.robot:
if qref is not None:
self.robot.SetActiveDOFValues(qref)
if checkcollision:
qsol = self.manip.FindIKSolution(T, ikfilter_checkcollision)
else:
qsol = self.manip.FindIKSolution(T, ikfilter_ignorecollision)
if qsol is not None:
# IKFast works. Return the IKFast solution directly.
return qsol
# Here IKFast does not return anything. It is either that a
# solution exists but IKFast fails or no solution exists.
targetpose = orpy.poseFromMatrix(T)
for i in xrange(self._ntrials):
# Perturb the desired T
Tnew = PerturbT(T)
with self.robot:
if qref is not None:
self.robot.SetActiveDOFValues(qref)
if checkcollision:
qinit = self.manip.FindIKSolution(Tnew, ikfilter_checkcollision)
else:
qinit = self.manip.FindIKSolution(Tnew, ikfilter_ignorecollision)
if qinit is None:
continue
# Since qinit is assumably close to a real solution (if
# one exists), we set max_it to be only 20.
[reached, _, qsol] = self.diffiksolver.solve\
(targetpose, qinit, dt=1.0, max_it=20, conv_tol=1e-8, checkcollision=checkcollision)
if not reached:
continue
# message = "Desired transformation reached"
# self.logger.info(message)
return qsol
message = "Failed to find an IK solution after {0} trials".format(self._ntrials)
self.logger.info(message)
return None
def sample_traj(self, traj_obj):
spec = traj_obj.GetConfigurationSpecification()
traj = []
step = traj_obj.GetDuration() / self.params.n_steps
for i in range(self.params.n_steps):
data = traj_obj.Sample(i * step)
T = spec.ExtractTransform(None, data, self.robot)
pose = rave.poseFromMatrix(T) # wxyz, xyz
euler = transformations.euler_from_quaternion(np.r_[pose[1:4], pose[0]])
traj.append(np.r_[pose[4:7], euler[0:3]])
with self.robot:
if check_traj.traj_is_safe(traj, self.robot):
return traj, traj_obj.GetDuration()
return None, None
def main():
### Parameters ###
ENV_FILE = "../data/pr2_table.env.xml"
MANIP_NAME = "leftarm"
N_STEPS = 15
LINK_NAME = "l_gripper_tool_frame"
XYZ_TARGET = [0.5, 0, 0.9]
QUAT_TARGET = [1, 0, 0, 0]
INTERACTIVE = True
##################
### Env setup ####
env = rave.RaveGetEnvironment(1)
if env is None:
env = rave.Environment()
env.StopSimulation()
atexit.register(rave.RaveDestroy)
env.Load(ENV_FILE)
robot = env.GetRobots()[0]
manip = robot.GetManipulator(MANIP_NAME)
robot.SetDOFValues(np.zeros(len(manip.GetArmIndices())), manip.GetArmIndices())
##################
T_start = robot.GetLink(LINK_NAME).GetTransform()
quat_xyz = rave.poseFromMatrix(T_start)
quat_start = quat_xyz[:4]
xyz_start = quat_xyz[4:7]
request = move_arm_straight_request(
manip,
N_STEPS,
LINK_NAME,
xyz_start=xyz_start,
xyz_end=XYZ_TARGET,
quat_start=quat_start,
quat_end=QUAT_TARGET,
start_joints=robot.GetDOFValues(manip.GetArmJoints()),
)
s = json.dumps(request)
print "REQUEST:", s
trajoptpy.SetInteractive(INTERACTIVE)
prob = trajoptpy.ConstructProblem(s, env)
result = trajoptpy.OptimizeProblem(prob)
def _get_OpenRaveFK(self, config, link_name):
"""
Calculate the forward kinematics using openRAVE for use in cost evaluation.
Params
---
config: Robot joint configuration (3,) numpy array
link_name: Name of the link to calculate forward kinematics for
"""
q = config.tolist()
self.robot.SetDOFValues(q + [0.0, 0.0, 0.0])
eef_link = self.robot.GetLink(link_name)
if eef_link is None:
rospy.logerror("Error: end-effector \"{}\" does not exist".format(
self.params["eef_link_name"]))
raise ValueError(
"Error: end-effector \"{}\" does not exist".format(
self.params["eef_link_name"]))
eef_pose = openravepy.poseFromMatrix(eef_link.GetTransform())
return np.array([eef_pose[4], eef_pose[5], eef_pose[6]])
def chomp_plan(robot, group_name, active_joint_names, active_affine,
target_dof_values, init_trajs):
datadir = 'chomp_data'
n_points = args.n_steps
assert active_affine == 0 or active_affine == 11
use_base = active_affine == 11
saver = openravepy.RobotStateSaver(robot)
target_base_pose = None
if use_base:
with robot:
robot.SetActiveDOFValues(target_dof_values)
target_base_pose = openravepy.poseFromMatrix(robot.GetTransform())
robot.SetActiveDOFs(
robot.GetActiveDOFIndices(),
0) # turn of affine dofs; chomp takes that separately
target_dof_values = target_dof_values[:-3] # strip off the affine part
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Warn)
m_chomp = get_chomp_module(robot.GetEnv())
env_hash = hash_env(robot.GetEnv())
if active_affine != 0:
env_hash += "_aa" + str(active_affine)
# load distance field
j1idxs = [m.GetArmIndices()[0] for m in robot.GetManipulators()]
for link in robot.GetLinks():
for j1idx in j1idxs:
if robot.DoesAffect(j1idx, link.GetIndex()):
link.Enable(False)
try:
aabb_padding = 1.0 if not use_base else 3.0 # base problems should need a distance field over a larger volume
m_chomp.computedistancefield(kinbody=robot,
aabb_padding=aabb_padding,
cache_filename='%s/chomp-sdf-%s.dat' %
(datadir, env_hash))
except Exception, e:
print 'Exception in computedistancefield:', repr(e)
def resolve_clusters(env, pattern_poses):
num_bodies = sum([1 for body in env.GetBodies() if is_object(body)])
if num_bodies < len(pattern_poses):
rospy.logwarn("Detected fewer objects than markers!")
if num_bodies > len(pattern_poses):
rospy.logwarn("Detected more objects than markers!")
listener = tf.TransformListener()
detection_results = []
for body in env.GetBodies():
if is_object(body):
""" REMOVED BECAUSE BBOXES ALREADY COMPUTED IN /BASE_LINK FRAME
bbox_centroid_in = PointStamped()
bbox_centroid_in.header.frame_id = '/head_mount_link'
bbox_centroid_in.point.x, bbox_centroid_in.point.y, bbox_centroid_in.point.z = openravepy.poseFromMatrix(body.GetTransform())[4:7]
bbox_centroid = PointStamped()
tf.TransformListener().transformPoint('/base_link', bbox_centroid_in, bbox_centroid)"""
bbox_centroid = openravepy.poseFromMatrix(body.GetTransform())[4:7]
min_dist, min_dist_id, min_dist_pose = -1, None, None
for (pattern_id, frame_id, pattern_pose) in pattern_poses:
pattern_pose_in = PoseStamped()
pattern_pose_in.header.frame_id = frame_id
pattern_pose_in.pose = pattern_pose.pose
listener.waitForTransform("/base_link", frame_id, rospy.Time(0), rospy.Duration(5.0))
pattern_pose_out = listener.transformPose("/base_link", pattern_pose_in)
curr_dist = np.linalg.norm(np.array(bbox_centroid) - \
np.array((pattern_pose_out.pose.position.x, pattern_pose_out.pose.position.y, pattern_pose_out.pose.position.z)))
if min_dist == -1 or curr_dist < min_dist:
min_dist = curr_dist
min_dist_id = pattern_id
min_dist_pose = pattern_pose_out
detection_results.append((get_pattern_name(min_dist_id), min_dist_pose, body.GetName()))
return detection_results
def chomp_plan(robot, group_name, active_joint_names, active_affine, target_dof_values, init_trajs):
datadir = 'chomp_data'
n_points = args.n_steps
assert active_affine == 0 or active_affine == 11
use_base = active_affine == 11
saver = openravepy.RobotStateSaver(robot)
target_base_pose = None
if use_base:
with robot:
robot.SetActiveDOFValues(target_dof_values)
target_base_pose = openravepy.poseFromMatrix(robot.GetTransform())
robot.SetActiveDOFs(robot.GetActiveDOFIndices(), 0) # turn of affine dofs; chomp takes that separately
target_dof_values = target_dof_values[:-3] # strip off the affine part
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Warn)
m_chomp = get_chomp_module(robot.GetEnv())
env_hash = hash_env(robot.GetEnv())
if active_affine != 0:
env_hash += "_aa" + str(active_affine)
# load distance field
j1idxs = [m.GetArmIndices()[0] for m in robot.GetManipulators()]
for link in robot.GetLinks():
for j1idx in j1idxs:
if robot.DoesAffect(j1idx, link.GetIndex()):
link.Enable(False)
try:
aabb_padding = 1.0 if not use_base else 3.0 # base problems should need a distance field over a larger volume
m_chomp.computedistancefield(kinbody=robot, aabb_padding=aabb_padding,
cache_filename='%s/chomp-sdf-%s.dat' % (datadir, env_hash))
except Exception, e:
print 'Exception in computedistancefield:', repr(e)
def plan_follow_traj(robot, manip_name, ee_link, new_hmats, old_traj):
handles = []
def animate_traj(traj, robot):
with robot:
viewer = trajoptpy.GetViewer(robot.GetEnv())
for i, traj_mats in enumerate(zip(traj, new_hmats)):
dofs = traj_mats[0]
mat = traj_mats[1]
print mat
robot.SetDOFValues(
dofs,
robot.GetManipulator(manip_name).GetArmIndices())
colors = [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1)]
for i in range(3):
point = mat[:3, 3]
axis = mat[:3, i] / 10
handles.append(
Globals.env.drawarrow(p1=point,
p2=point + axis,
linewidth=0.004,
color=colors[i]))
viewer.Idle()
n_steps = len(new_hmats)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == 7
arm_inds = robot.GetManipulator(manip_name).GetArmIndices()
ee_linkname = ee_link.GetName()
init_traj = old_traj.copy()
#animate_traj(init_traj, robot)
#init_traj[0] = robot.GetDOFValues(arm_inds)
request = {
"basic_info": {
"n_steps": n_steps,
"manip": manip_name,
"start_fixed": False
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [1]
}
}, {
"type": "collision",
"params": {
"coeffs": [10],
"dist_pen": [0.005]
}
}],
"constraints": [],
"init_info": {
"type": "given_traj",
"data": [x.tolist() for x in init_traj]
}
}
poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
for (i_step, pose) in enumerate(poses):
request["costs"].append({
"type": "pose",
"params": {
"xyz": pose[4:7].tolist(),
"wxyz": pose[0:4].tolist(),
"link": ee_linkname,
"timestep": i_step,
"pos_coeffs": [20, 20, 20],
"rot_coeff": [20, 20, 20]
}
})
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(
s, Globals.env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
#animate_traj(traj, robot)
saver = openravepy.RobotStateSaver(robot)
pos_errs = []
for i_step in xrange(1, n_steps):
row = traj[i_step]
robot.SetDOFValues(row, arm_inds)
tf = ee_link.GetTransform()
pos = tf[:3, 3]
pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
pos_errs.append(pos_err)
pos_errs = np.array(pos_errs)
print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
manip_name, pos_errs.max(), pos_errs)
return traj
def mat_to_base_pose(mat):
pose = openravepy.poseFromMatrix(mat)
x = pose[4]
y = pose[5]
rot = openravepy.axisAngleFromRotationMatrix(mat)[2]
return x, y, rot
def get_position_from_matrix(transform):
return tuple(openravepy.poseFromMatrix(transform)[4:])
def generate_grasping_traj(self, obj, grasp_pose_list, collisionfree=True):
for grasp_pose, pre_grasp_pose in grasp_pose_list:
# find IK for pregrasp
if collisionfree:
init_joints1 = self.manip.FindIKSolution(pre_grasp_pose,
openravepy.IkFilterOptions.CheckEnvCollisions)
else:
init_joints1 = self.manip.FindIKSolution(pre_grasp_pose,
openravepy.IkFilterOptions.IgnoreEndEffectorCollisions)
with self.env:
# find IK for grasp
self.env.Remove(obj)
if collisionfree:
init_joints2 = self.manip.FindIKSolution(grasp_pose,
openravepy.IkFilterOptions.CheckEnvCollisions)
else:
init_joints2 = self.manip.FindIKSolution(grasp_pose,
openravepy.IkFilterOptions.IgnoreEndEffectorCollisions)
self.env.AddKinBody(obj)
if (init_joints1 is None) or (init_joints2 is None):
continue
# find traj for pregrasp
gripper_pose1 = openravepy.poseFromMatrix(pre_grasp_pose).tolist()
xyz_target1 = gripper_pose1[4:7]
# quaternions are rotated by pi/2 around y for some reason...
quat_target1 = openravepy.quatMultiply(gripper_pose1[:4],
(0.7071, 0, -0.7071, 0)).tolist()
traj1, collisions1 = self.traj_generator.traj_from_pose(
xyz_target1, quat_target1,
collisionfree=collisionfree, joint_targets=init_joints1.tolist())
if traj1 is None:
continue
with self.env:
# find trajectory to grasp
orig_values = self.robot.GetDOFValues(
self.robot.GetManipulator('rightarm').GetArmIndices())
self.robot.SetDOFValues(traj1[-1],
self.robot.GetManipulator('rightarm').GetArmIndices())
gripper_pose2 = openravepy.poseFromMatrix(grasp_pose).tolist()
xyz_target2 = gripper_pose2[4:7]
# quaternions are rotated by pi/2 around y for some reason...
quat_target2 = openravepy.quatMultiply(gripper_pose2[:4],
(0.7071, 0, -0.7071, 0)).tolist()
self.env.Remove(obj)
traj2, collisions2 = self.traj_generator.traj_from_pose(
xyz_target2, quat_target2, n_steps=2,
collisionfree=collisionfree, joint_targets=init_joints2.tolist())
self.env.AddKinBody(obj)
# reset
self.robot.SetDOFValues(orig_values,
self.robot.GetManipulator('rightarm').GetArmIndices())
if traj2 is None:
continue
collisions = collisions1.union(collisions2)
if obj in collisions:
collisions.remove(obj)
if self.unmovable_objects.intersection(collisions):
continue
return traj1.tolist() + traj2.tolist(), collisions
return None, set()
init_joint_target.tolist() # need to convert numpy array to list
}
# END init
}
if args.position_only:
request["constraints"][0]["params"]["rot_coeffs"] = [0, 0, 0]
s = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(
s, env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
print result
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
assert traj_is_safe(result.GetTraj(),
robot) # Check that trajectory is collision free
# Now we'll check to see that the final constraint was satisfied
robot.SetActiveDOFValues(result.GetTraj()[-1])
posevec = openravepy.poseFromMatrix(
robot.GetLink("r_gripper_tool_frame").GetTransform())
quat, xyz = posevec[0:4], posevec[4:7]
quat *= np.sign(quat.dot(quat_target))
if args.position_only:
assert (quat - quat_target).max() > 1e-3
else:
assert (quat - quat_target).max() < 1e-3
def plan_follow_trajs(robot,
manip_name,
ee_link_names,
ee_trajs,
old_traj,
no_collision_cost_first=False,
use_collision_cost=True,
start_fixed=False,
joint_vel_limits=None,
beta_pos=settings.BETA_POS,
beta_rot=settings.BETA_ROT,
gamma=settings.GAMMA):
orig_dof_inds = robot.GetActiveDOFIndices()
orig_dof_vals = robot.GetDOFValues()
n_steps = len(ee_trajs[0])
dof_inds = sim_util.dof_inds_from_name(robot, manip_name)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == len(dof_inds)
assert len(ee_link_names) == len(ee_trajs)
if no_collision_cost_first:
init_traj, _, _ = plan_follow_trajs(robot,
manip_name,
ee_link_names,
ee_trajs,
old_traj,
no_collision_cost_first=False,
use_collision_cost=False,
start_fixed=start_fixed,
joint_vel_limits=joint_vel_limits,
beta_pos=beta_pos,
beta_rot=beta_rot,
gamma=gamma)
else:
init_traj = old_traj.copy()
if start_fixed:
init_traj = np.r_[robot.GetDOFValues(dof_inds)[None, :], init_traj[1:]]
sim_util.unwrap_in_place(init_traj, dof_inds)
init_traj += robot.GetDOFValues(dof_inds) - init_traj[0, :]
request = {
"basic_info": {
"n_steps": n_steps,
"manip": manip_name,
"start_fixed": start_fixed
},
"costs": [
{
"type": "joint_vel",
"params": {
"coeffs": [gamma / (n_steps - 1)]
}
},
],
"constraints": [],
"init_info": {
"type": "given_traj",
"data": [x.tolist() for x in init_traj]
}
}
if use_collision_cost:
request["costs"].append({
"type": "collision",
"params": {
"continuous": True,
"coeffs": [
1000
], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
"dist_pen": [
0.025
] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
}
})
if joint_vel_limits is not None:
request["constraints"].append({
"type": "joint_vel_limits",
"params": {
"vals": joint_vel_limits,
"first_step": 0,
"last_step": n_steps - 1
}
})
for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj]
for (i_step, pose) in enumerate(poses):
if start_fixed and i_step == 0:
continue
request["costs"].append({
"type": "pose",
"params": {
"xyz": pose[4:7].tolist(),
"wxyz": pose[0:4].tolist(),
"link": ee_link_name,
"timestep": i_step,
"pos_coeffs": [np.sqrt(beta_pos / n_steps)] * 3,
"rot_coeffs": [np.sqrt(beta_rot / n_steps)] * 3
}
})
s = json.dumps(request)
with openravepy.RobotStateSaver(robot):
orig_dof_vals
with util.suppress_stdout():
prob = trajoptpy.ConstructProblem(s, robot.GetEnv(
)) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
pose_costs = np.sum([
cost_val for (cost_type, cost_val) in result.GetCosts()
if cost_type == "pose"
])
pose_err = []
with openravepy.RobotStateSaver(robot):
for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
ee_link = robot.GetLink(ee_link_name)
for (i_step, hmat) in enumerate(ee_traj):
if start_fixed and i_step == 0:
continue
robot.SetDOFValues(traj[i_step], dof_inds)
new_hmat = ee_link.GetTransform()
pose_err.append(
openravepy.poseFromMatrix(
mu.invertHmat(hmat).dot(new_hmat)))
pose_err = np.asarray(pose_err)
pose_costs2 = (beta_rot / n_steps) * np.square(pose_err[:, 1:4]).sum() + (
beta_pos / n_steps) * np.square(pose_err[:, 4:7]).sum()
joint_vel_cost = np.sum([
cost_val for (cost_type, cost_val) in result.GetCosts()
if cost_type == "joint_vel"
])
joint_vel_err = np.diff(traj, axis=0)
joint_vel_cost2 = (gamma / (n_steps - 1)) * np.square(joint_vel_err).sum()
sim_util.unwrap_in_place(traj, dof_inds)
joint_vel_err = np.diff(traj, axis=0)
collision_costs = [
cost_val for (cost_type, cost_val) in result.GetCosts()
if "collision" in cost_type
]
if len(collision_costs) > 0:
collision_err = np.asarray(collision_costs)
collision_costs = np.sum(collision_costs)
obj_value = np.sum(
[cost_val for (cost_type, cost_val) in result.GetCosts()])
print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed")
print "{:>15} | {:>10}".format("COSTS", "-" * 23)
print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost,
joint_vel_cost2)
if np.isscalar(collision_costs):
print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)",
collision_costs, "-")
print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs,
pose_costs2)
print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-")
print ""
print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max")
print "{:>15} | {:>10}".format("ERRORS", "-" * 23)
print "{:>15} | {:>10,.4} | {:>10,.4}".format(
"joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()),
np.rad2deg(np.abs(joint_vel_err).max()))
if np.isscalar(collision_costs):
print "{:>15} | {:>10,.4} | {:>10,.4}".format(
"collision(s)",
np.abs(-collision_err).min(),
np.abs(-collision_err).max())
print "{:>15} | {:>10,.4} | {:>10,.4}".format(
"rot pose(s)",
np.abs(pose_err[:, 1:4]).min(),
np.abs(pose_err[:, 1:4]).max())
print "{:>15} | {:>10,.4} | {:>10,.4}".format(
"trans pose(s)",
np.abs(pose_err[:, 4:7]).min(),
np.abs(pose_err[:, 4:7]).max())
print ""
# make sure this function doesn't change state of the robot
assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices())
assert not np.any(orig_dof_vals - robot.GetDOFValues())
return traj, obj_value, pose_costs
def test_move_pr2_with_obj_wrt_obj():
env = cans_world_env()
obj17 = env.GetKinBody('object17')
tran = obj17.GetTransform()
tran[0,3] = .49268
tran[1,3] = -.51415
obj17.SetTransform(tran)
hl_plan = TestDomain(env)
move_env = env.CloneSelf(1) # clones objects in the environment
move_pr2 = move_env.GetKinBody('pr2')
move_pr2.SetActiveDOFs(move_pr2.GetManipulator('rightarm').GetArmIndices())
move_pr2.SetDOFValues([0.3],
[move_pr2.GetJoint("torso_lift_joint").GetDOFIndex()])
robot = PR2('pr2')
robot.tuck_arms(env)
robot._set_active_dofs(env, ['rightarm'])
pose = robot.get_pose(env)
hl_plan = HLPlan(env)
obj = Obj('object17')
K = 7
# obj_pose = np.array([[0.],[0.],[0.]])
# obj.set_pose(env, obj_pose)
# start = HLParam("pos", (self.K, 1), is_var=False, value=robot.get_pose(env))
start_pose = np.array([[-1.4352011 ],\
[ 0.63522797],\
[-1.19804084],\
[-1.55807855],\
[-0.9962505 ],\
[-0.75279673],\
[-1.13988334]])
end_pose = np.array([[-1.28868338],\
[ 0.53921863],\
[-1.41293145],\
[-1.67185359],\
[-0.74117023],\
[-0.73603889],\
[-1.29004773]])
robot.set_pose(env, start_pose)
# traj_val = lininterp(start_pose, end_pose, 10)
traj_val = lininterp(start_pose, end_pose, 2)
# getting grasp pose
robot_body = robot.get_env_body(env)
rarm = robot_body.GetManipulator('rightarm')
# import ipdb; ipdb.set_trace()
W_T_EE = rarm.GetEndEffectorTransform()
EE_T_W = np.linalg.inv(W_T_EE)
W_T_O = obj17.GetTransform()
EE_T_O = np.dot(EE_T_W, W_T_O)
EE_T_O_pose = poseFromMatrix(EE_T_O)
# gp = HLParam("gp", (7, 1), is_var=False, value=EE_T_O_pose)
gp_val = np.array([0,0,.125]).reshape((3,1))
gp = HLParam("gp", (3, 1), is_var=False, value=gp_val)
# start_pose = np.array([[-1.07265580e+00], [5.17875957e-01], [-1.30000000e+00], \
# [-1.08244363e+00], [-1.04143184e+00], [-9.06938766e-02], [-5.38403045e-01]])
# obj trajectory to follow
# obj_start = poseFromMatrix(tran).reshape((7,1))
obj_start = obj_pose_from_transform(tran).reshape((6,1))
end_tran = tran.copy()
end_tran[1, 3] = -0.34
obj_end = obj_pose_from_transform(end_tran).reshape((6,1))
# print "obj_start:", obj_start
# print "obj_end:", obj_end
# obj_traj = lininterp(obj_start, obj_end, 10)
obj_traj = lininterp(obj_start, obj_end, 2)
start = HLParam("start", (K, 1), is_var=False, value=start_pose)
end = HLParam("end", (K, 1), is_var=False, value=end_pose)
# obj_pos = HLParam("obj_pos", (3, 1), is_var=False, value=obj.get_pose(env))
move = PR2ObjMove(0, hl_plan, move_env, robot, start, end, traj_val, obj, gp)
hlas = [move]
ll_prob = LLProb(hlas)
ll_prob.solve()
import ipdb; ipdb.set_trace()
def pose_from_trans(trans):
return poseFromMatrix(trans)
def plan_follow_traj(
robot,
manip_name,
ee_link,
new_hmats,
old_traj,
rope_cloud=None,
rope_constraint_thresh=0.01,
end_pose_constraint=False,
):
n_steps = len(new_hmats)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == 7
arm_inds = robot.GetManipulator(manip_name).GetArmIndices()
ee_linkname = ee_link.GetName()
init_traj = old_traj.copy()
# init_traj[0] = robot.GetDOFValues(arm_inds)
end_pose = openravepy.poseFromMatrix(new_hmats[-1])
request = {
"basic_info": {"n_steps": n_steps, "manip": manip_name, "start_fixed": False},
"costs": [
{"type": "joint_vel", "params": {"coeffs": [n_steps / 5.0]}},
{"type": "collision", "params": {"coeffs": [1], "dist_pen": [0.01]}},
],
"constraints": [],
"init_info": {"type": "given_traj", "data": [x.tolist() for x in init_traj]},
}
request["costs"] += [
{"type": "collision", "name": "cont_coll", "params": {"coeffs": [50], "dist_pen": [0.05]}},
{"type": "collision", "name": "col", "params": {"continuous": False, "coeffs": [20], "dist_pen": [0.02]}},
]
# impose that the robot goes to final ee tfm at last ts
# the constraint works only when the arm is the 'grasp' arm; otherwise only cost is added
# if end_pose_constraint or not is_fake_motion(new_hmats, 0.1):
# hack to avoid missing grasp
if rope_cloud != None:
closest_point = find_closest_point(rope_cloud, end_pose[4:7])
dist = np.linalg.norm(end_pose[4:7] - closest_point)
if dist > rope_constraint_thresh and dist < 0.05:
end_pose[4:7] = closest_point
redprint("grasp hack is active, dist = %f" % dist)
else:
blueprint("grasp hack is inactive, dist = %f" % dist)
# raw_input()
request["constraints"] += [
{
"type": "pose",
"params": {
"xyz": end_pose[4:7].tolist(),
"wxyz": end_pose[0:4].tolist(),
"link": ee_linkname,
"pos_coeffs": [100, 100, 100],
"rot_coeffs": [10, 10, 10],
},
}
]
poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
for (i_step, pose) in enumerate(poses):
request["costs"].append(
{
"type": "pose",
"params": {
"xyz": pose[4:7].tolist(),
"wxyz": pose[0:4].tolist(),
"link": ee_linkname,
"timestep": i_step,
"pos_coeffs": [100, 100, 100],
"rot_coeffs": [10, 10, 10],
},
}
)
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
saver = openravepy.RobotStateSaver(robot)
pos_errs = []
with saver:
for i_step in xrange(1, n_steps):
row = traj[i_step]
robot.SetDOFValues(row, arm_inds)
tf = ee_link.GetTransform()
pos = tf[:3, 3]
pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
pos_errs.append(pos_err)
pos_errs = np.array(pos_errs)
print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
manip_name,
pos_errs.max(),
pos_errs,
)
return traj
def pose_from_trans(trans): return poseFromMatrix(trans)
def track(self, Tinit, direction, length, trackersteplength=0.0001, qinit=None):
"""
Track a straight line in Cartesian space (3D).
Tinit -- the initial transforamtion of the manipulator. Along
the path, the manipulator will stay pointing along
Tinit[0:3, 2].
direction -- a 3D-vector of workspace direction. The path is
therefore starting from Tinit[0:3, 3] and ending at
Tinit[0:3, 3] + length*direction.
length -- the length to go along direction
trackersteplength -- a step size (in m.) for the tracker
qinit -- an initial ik solution corresponding to Tinit.
Note: Tinit[0:3, 2] and direction may be different from each other.
Return a list of status and waypointslist.
"""
if qinit is None:
qinit = self._robustiksolver.FindIKSolution(Tinit)
if qinit is None:
message = "Failed to find an initial IK solution"
self.logger.info(message)
return [False, []]
nsteps = int(length/trackersteplength)
assert(nsteps > 0) # check soundness
direction = direction / np.linalg.norm(direction)
M = np.array(Tinit) # dummy transformation matrix
waypointslist = [] # solution
stepvector = trackersteplength * direction
waypointslist.append(qinit)
qprev = np.array(qinit)
for i in xrange(nsteps):
M[0:3, 3] += stepvector
targetpose = orpy.poseFromMatrix(M)
[reached, _, qsol] = self._robustiksolver.diffiksolver.solve\
(targetpose, qprev, dt=1.0, max_it=100, conv_tol=1e-8)
if not reached:
message = 'Failed to track the path at step {0}'.format(i + 1)
self.logger.info(message)
if self._printextrainfo:
print 'qprev = np.' + repr(qprev)
print 'Ttarget = np.' + repr(M)
return [False, []]
waypointslist.append(qsol)
qprev = np.array(qsol)
if not np.allclose(M[0:3, 3], Tinit[0:3, 3] + length*direction):
M[0:3, 3] = Tinit[0:3, 3] + length*direction
targetpose = orpy.poseFromMatrix(M)
[reached, _, qsol] = self._robustiksolver.diffiksolver.solve\
(targetpose, qprev, dt=1.0, max_it=100, conv_tol=1e-8)
if not reached:
message = 'Failed to track the path at the last step'
self.logger.info(message)
if self._printextrainfo:
print 'qprev = np.' + repr(qprev)
print 'Ttarget = np.' + repr(M)
return [False, []]
waypointslist.append(qsol)
qprev = np.array(qsol)
return [True, waypointslist]
def xyzQuatFromMatrix(T):
wxyz_xyz = rave.poseFromMatrix(T)
return wxyz_xyz[4:7], wxyz_xyz[0:4]
def plan_follow_traj(robot, manip_name, ee_link, new_hmats, old_traj):
n_steps = len(new_hmats)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == 7
arm_inds = robot.GetManipulator(manip_name).GetArmIndices()
ee_linkname = ee_link.GetName()
init_traj = old_traj.copy()
#init_traj[0] = robot.GetDOFValues(arm_inds)
request = {
"basic_info": {
"n_steps": n_steps,
"manip": manip_name,
"start_fixed": False
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [n_steps / 5.]
}
}, {
"type": "collision",
"params": {
"coeffs": [10],
"dist_pen": [0.01]
}
}],
"constraints": [],
"init_info": {
"type": "given_traj",
"data": [x.tolist() for x in init_traj]
}
}
poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
for (i_step, pose) in enumerate(poses):
request["costs"].append({
"type": "pose",
"params": {
"xyz": pose[4:7].tolist(),
"wxyz": pose[0:4].tolist(),
"link": ee_linkname,
"timestep": i_step,
"pos_coeffs": [10, 10, 10],
"rot_coeffs": [10, 10, 10]
}
})
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(
s, robot.GetEnv()) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
saver = openravepy.RobotStateSaver(robot)
pos_errs = []
for i_step in xrange(1, n_steps):
row = traj[i_step]
robot.SetDOFValues(row, arm_inds)
tf = ee_link.GetTransform()
pos = tf[:3, 3]
pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
pos_errs.append(pos_err)
pos_errs = np.array(pos_errs)
print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
manip_name, pos_errs.max(), pos_errs)
return traj
def mat_to_base_pose_2D(mat):
pose = poseFromMatrix(mat)
x = pose[4]
y = pose[5]
return np.array([x,y])
def FindIKSolutions(self, point, direction, checkcollision=True):
"""
point -- a 3D vector
direction -- a 3D 'unit' vector
"""
self.ActivateIKSolver()
# Use IKFast
ikparam = orpy.IkParameterization(orpy.Ray(point, direction), iktype5D)
if checkcollision:
sols = self.manip.FindIKSolutions(ikparam, ikfilter_checkcollision)
else:
sols = self.manip.FindIKSolutions(ikparam, ikfilter_ignorecollision)
if len(sols) > 0:
# IKFast works. Return the IKFast solution directly.
return sols
# Compute an initial rotation
z = np.array(direction).reshape((3, 1))
x = PerpendicularTo(z).reshape((3, 1))
y = np.cross(z.T, x.T).reshape((3, 1))
R = np.hstack([x, y, z])
nattempts = 3
sols = []
for i in xrange(nattempts):
# Sample a rotation
theta = (2.0*rng.random() - 1.0) * np.pi
cos = np.cos(theta)
sin = np.sin(theta)
Rtheta = np.array([[ cos, -sin, 0.0],
[ sin, cos, 0.0],
[ 0.0, 0.0, 1.0]])
Rnew = np.dot(R, Rtheta)
T = CombineRotationTranslation(Rnew, point) # desired transformation
targetpose = orpy.poseFromMatrix(T) # desired pose
# Add some perturbation
Tnew = PerturbT(T)
# Let IKFast provide an initial solution
ray = orpy.Ray(Tnew[0:3, 3], Tnew[0:3, 2]/np.linalg.norm(Tnew[0:3, 2]))
ikparam = orpy.IkParameterization(ray, iktype5D)
if checkcollision:
sols_init = self.manip.FindIKSolutions(ikparam, ikfilter_checkcollision)
else:
sols_init = self.manip.FindIKSolutions(ikparam, ikfilter_ignorecollision)
if len(sols_init) == 0:
continue
for qinit in sols_init:
# Since qinit is assumably close to a real solution (if
# one exists), we set max_it to be only 20.
[reached, _, qsol] = self.diffiksolver.solve\
(targetpose, qinit, dt=1.0, max_it=20, conv_tol=1e-8, checkcollision=checkcollision)
if reached:
sols.append(qsol)
if len(sols) == 0:
message = "Failed to find an IK solution after {0} trials".format(self._ntrials)
self.logger.info(message)
return sols
def plan_follow_traj(robot, manip_name, ee_link, new_hmats, old_traj):
n_steps = len(new_hmats)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == 7
arm_inds = robot.GetManipulator(manip_name).GetArmIndices()
ee_linkname = ee_link.GetName()
init_traj = old_traj.copy()
#init_traj[0] = robot.GetDOFValues(arm_inds)
request = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : manip_name,
"start_fixed" : False
},
"costs" : [
{
"type" : "joint_vel",
"params": {"coeffs" : [1]}
},
{
"type" : "collision",
"params" : {"coeffs" : [10],"dist_pen" : [0.005]}
}
],
"constraints" : [
],
"init_info" : {
"type":"given_traj",
"data":[x.tolist() for x in init_traj]
}
}
poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
for (i_step,pose) in enumerate(poses):
request["costs"].append(
{"type":"pose",
"params":{
"xyz":pose[4:7].tolist(),
"wxyz":pose[0:4].tolist(),
"link":ee_linkname,
"timestep":i_step,
"pos_coeffs":[20,20,20],
"rot_coeff":[20,20,20]
}
})
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, Globals.env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
saver = openravepy.RobotStateSaver(robot)
pos_errs = []
for i_step in xrange(1,n_steps):
row = traj[i_step]
robot.SetDOFValues(row, arm_inds)
tf = ee_link.GetTransform()
pos = tf[:3,3]
pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
pos_errs.append(pos_err)
pos_errs = np.array(pos_errs)
print "planned trajectory for %s. max position error: %.3f. all position errors: %s"%(manip_name, pos_errs.max(), pos_errs)
return traj
def xyzQuatFromMatrix(T):
wxyz_xyz = rave.poseFromMatrix(T)
return wxyz_xyz[4:7], wxyz_xyz[0:4]
def plan_fullbody(
robot,
env,
new_hmats_by_bodypart,
old_traj_by_bodypart,
end_pose_constraints,
rope_cloud=None,
rope_constraint_thresh=0.01,
allow_base=True,
init_data=None,
):
collision_free = True
for part_name in new_hmats_by_bodypart:
n_steps = len(new_hmats_by_bodypart[part_name])
break
state = np.random.RandomState(None)
init_dofs = []
if "base" in old_traj_by_bodypart.keys():
if "rightarm" in old_traj_by_bodypart.keys():
init_dofs += old_traj_by_bodypart["rightarm"].tolist()
else:
init_dofs += robot.GetDOFValues(robot.GetManipulator("rightarm").GetArmIndices()).tolist()
if "leftarm" in old_traj_by_bodypart.keys():
init_dofs += old_traj_by_bodypart["leftarm"].tolist()
else:
init_dofs += robot.GetDOFValues(robot.GetManipulator("leftarm").GetArmIndices()).tolist()
init_dofs += old_traj_by_bodypart["base"]
else:
arm_dofs = []
if "rightarm" in old_traj_by_bodypart.keys():
arm_dofs = old_traj_by_bodypart["rightarm"]
if "leftarm" in old_traj_by_bodypart.keys():
arm_dofs = np.c_[arm_dofs, old_traj_by_bodypart["leftarm"]]
init_dofs = arm_dofs
if allow_base:
x, y, rot = mat_to_base_pose(robot.GetTransform())
base_dofs = np.c_[x, y, rot]
init_dofs = np.c_[init_dofs, np.repeat(base_dofs, init_dofs.shape[0], axis=0)]
init_dofs = init_dofs.tolist()
bodyparts = new_hmats_by_bodypart.keys()
if "base" in bodyparts: # problem
bodyparts += ["rightarm", "leftarm"]
elif allow_base:
bodyparts.append("base")
costs = []
constraints = []
if "base" in bodyparts:
if old_traj_by_bodypart is None:
raise Exception("Base motion planning must have init_dofs")
n_dofs = len(init_dofs[0])
cost_coeffs = n_dofs * [5]
cost_coeffs[-1] = 500
cost_coeffs[-2] = 500
cost_coeffs[-3] = 500
joint_vel_cost = {"type": "joint_vel", "params": {"coeffs": cost_coeffs}}
costs.append(joint_vel_cost)
else:
joint_vel_cost = {
"type": "joint_vel", # joint-space velocity cost
"params": {"coeffs": [5]}, # a list of length one is automatically expanded to a list of length n_dofs
}
costs.append(joint_vel_cost)
for manip, poses in new_hmats_by_bodypart.items():
if manip == "rightarm":
link = "r_gripper_tool_frame"
elif manip == "leftarm":
link = "l_gripper_tool_frame"
elif manip == "base":
link = "base_footprint"
if manip in ["rightarm", "leftarm"]:
if not end_pose_constraints[manip] and is_fake_motion(poses, 0.1):
continue
if manip in ["rightarm", "leftarm"]:
if end_pose_constraints[manip] or not is_fake_motion(poses, 0.1):
end_pose = openravepy.poseFromMatrix(poses[-1])
if rope_cloud != None:
closest_point = find_closest_point(rope_cloud, end_pose[4:7])
dist = np.linalg.norm(end_pose[4:7] - closest_point)
if dist > rope_constraint_thresh and dist < 0.2:
end_pose[4:7] = closest_point
redprint("grasp hack is active, dist = %f" % dist)
constraints.append(
{
"type": "pose",
"params": {
"xyz": end_pose[4:7].tolist(),
"wxyz": end_pose[0:4].tolist(),
"link": link,
"pos_coeffs": [10, 10, 10],
"rot_coeffs": [10, 10, 10],
},
}
)
poses = [openravepy.poseFromMatrix(hmat) for hmat in poses]
for t, pose in enumerate(poses):
costs.append(
{
"type": "pose",
"params": {
"xyz": pose[4:].tolist(),
"wxyz": pose[:4].tolist(),
"link": link,
"pos_coeffs": [20, 20, 20],
"rot_coeffs": [20, 20, 20],
"timestep": t,
},
}
)
traj, total_cost = generate_traj(robot, env, costs, constraints, n_steps, init_dofs, collision_free)
bodypart_traj = traj_to_bodypart_traj(traj, bodyparts)
# Do multi init if base planning
if "base" in bodypart_traj and not allow_base:
waypoint_step = (n_steps - 1) // 2
env_min, env_max = get_environment_limits(env, robot)
current_dofs = robot.GetActiveDOFValues().tolist()
waypoint_dofs = list(current_dofs)
MAX_INITS = 10
n = 0
while True:
collisions = get_traj_collisions(bodypart_traj, robot)
if collisions == []:
break
n += 1
if n > MAX_INITS:
print "Max base multi init limit hit!"
return bodypart_traj, total_cost # return arbitrary traj with collisions
print "Base planning failed! Trying random init. Iteration {} of {}".format(n, MAX_INITS)
# randomly sample x, y within env limits + some padding to accommodate robot
padding = 5.0
env_min_x = env_min[0] - padding
env_min_y = env_min[1] - padding
env_max_x = env_max[0] + padding
env_max_y = env_max[1] + padding
waypoint_x = state.uniform(env_min_x, env_max_x)
waypoint_y = state.uniform(env_min_y, env_max_y)
waypoint_dofs[-3] = waypoint_x
waypoint_dofs[-2] = waypoint_y
init_data = np.empty((n_steps, robot.GetActiveDOF()))
init_data[: waypoint_step + 1] = mu.linspace2d(current_dofs, waypoint_dofs, waypoint_step + 1)
init_data[waypoint_step:] = mu.linspace2d(waypoint_dofs, init_dofs, n_steps - waypoint_step)
traj, total_cost = generate_traj(robot, env, costs, constraints, n_steps, collision_free, init_dofs, init_data)
bodypart_traj = traj_to_bodypart_traj(traj, bodyparts)
return bodypart_traj, total_cost
def mat_to_base_pose(mat):
pose = poseFromMatrix(mat)
x = pose[4]
y = pose[5]
rot = axisAngleFromRotationMatrix(mat)[2]
return np.array([x,y,rot])
def plan_follow_traj(robot, manip_name, ee_link, new_hmats, old_traj):
handles = []
def animate_traj(traj, robot):
with robot:
viewer = trajoptpy.GetViewer(robot.GetEnv())
for i, traj_mats in enumerate(zip(traj, new_hmats)):
dofs = traj_mats[0]
mat = traj_mats[1]
print mat
robot.SetDOFValues(dofs, robot.GetManipulator(manip_name).GetArmIndices())
colors = [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1)]
for i in range(3):
point = mat[:3, 3]
axis = mat[:3, i] / 10
handles.append(Globals.env.drawarrow(p1=point, p2=point + axis, linewidth=0.004, color=colors[i]))
viewer.Idle()
n_steps = len(new_hmats)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == 7
arm_inds = robot.GetManipulator(manip_name).GetArmIndices()
ee_linkname = ee_link.GetName()
init_traj = old_traj.copy()
# animate_traj(init_traj, robot)
# init_traj[0] = robot.GetDOFValues(arm_inds)
request = {
"basic_info": {"n_steps": n_steps, "manip": manip_name, "start_fixed": False},
"costs": [
{"type": "joint_vel", "params": {"coeffs": [1]}},
{"type": "collision", "params": {"coeffs": [10], "dist_pen": [0.005]}},
],
"constraints": [],
"init_info": {"type": "given_traj", "data": [x.tolist() for x in init_traj]},
}
poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
for (i_step, pose) in enumerate(poses):
request["costs"].append(
{
"type": "pose",
"params": {
"xyz": pose[4:7].tolist(),
"wxyz": pose[0:4].tolist(),
"link": ee_linkname,
"timestep": i_step,
"pos_coeffs": [20, 20, 20],
"rot_coeff": [20, 20, 20],
},
}
)
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, Globals.env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
# animate_traj(traj, robot)
saver = openravepy.RobotStateSaver(robot)
pos_errs = []
for i_step in xrange(1, n_steps):
row = traj[i_step]
robot.SetDOFValues(row, arm_inds)
tf = ee_link.GetTransform()
pos = tf[:3, 3]
pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
pos_errs.append(pos_err)
pos_errs = np.array(pos_errs)
print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
manip_name,
pos_errs.max(),
pos_errs,
)
return traj
def plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj,
no_collision_cost_first=False, use_collision_cost=True, start_fixed=False, joint_vel_limits=None,
beta_pos=settings.BETA_POS, beta_rot=settings.BETA_ROT, gamma=settings.GAMMA):
orig_dof_inds = robot.GetActiveDOFIndices()
orig_dof_vals = robot.GetDOFValues()
n_steps = len(ee_trajs[0])
dof_inds = sim_util.dof_inds_from_name(robot, manip_name)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == len(dof_inds)
assert len(ee_link_names) == len(ee_trajs)
if no_collision_cost_first:
init_traj, _, _ = plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj,
no_collision_cost_first=False, use_collision_cost=False, start_fixed=start_fixed, joint_vel_limits=joint_vel_limits,
beta_pos = beta_pos, beta_rot = beta_rot, gamma = gamma)
else:
init_traj = old_traj.copy()
if start_fixed:
init_traj = np.r_[robot.GetDOFValues(dof_inds)[None,:], init_traj[1:]]
sim_util.unwrap_in_place(init_traj, dof_inds)
init_traj += robot.GetDOFValues(dof_inds) - init_traj[0,:]
request = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : manip_name,
"start_fixed" : start_fixed
},
"costs" : [
{
"type" : "joint_vel",
"params": {"coeffs" : [gamma/(n_steps-1)]}
},
],
"constraints" : [
],
"init_info" : {
"type":"given_traj",
"data":[x.tolist() for x in init_traj]
}
}
if use_collision_cost:
request["costs"].append(
{
"type" : "collision",
"params" : {
"continuous" : True,
"coeffs" : [1000], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
"dist_pen" : [0.025] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
}
})
if joint_vel_limits is not None:
request["constraints"].append(
{
"type" : "joint_vel_limits",
"params": {"vals" : joint_vel_limits,
"first_step" : 0,
"last_step" : n_steps-1
}
})
for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj]
for (i_step,pose) in enumerate(poses):
if start_fixed and i_step == 0:
continue
request["costs"].append(
{"type":"pose",
"params":{
"xyz":pose[4:7].tolist(),
"wxyz":pose[0:4].tolist(),
"link":ee_link_name,
"timestep":i_step,
"pos_coeffs":[np.sqrt(beta_pos/n_steps)]*3,
"rot_coeffs":[np.sqrt(beta_rot/n_steps)]*3
}
})
s = json.dumps(request)
with openravepy.RobotStateSaver(robot):
orig_dof_vals
with util.suppress_stdout():
prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
pose_costs = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "pose"])
pose_err = []
with openravepy.RobotStateSaver(robot):
for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
ee_link = robot.GetLink(ee_link_name)
for (i_step, hmat) in enumerate(ee_traj):
if start_fixed and i_step == 0:
continue
robot.SetDOFValues(traj[i_step], dof_inds)
new_hmat = ee_link.GetTransform()
pose_err.append(openravepy.poseFromMatrix(mu.invertHmat(hmat).dot(new_hmat)))
pose_err = np.asarray(pose_err)
pose_costs2 = (beta_rot/n_steps) * np.square(pose_err[:,1:4]).sum() + (beta_pos/n_steps) * np.square(pose_err[:,4:7]).sum()
joint_vel_cost = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "joint_vel"])
joint_vel_err = np.diff(traj, axis=0)
joint_vel_cost2 = (gamma/(n_steps-1)) * np.square(joint_vel_err).sum()
sim_util.unwrap_in_place(traj, dof_inds)
joint_vel_err = np.diff(traj, axis=0)
collision_costs = [cost_val for (cost_type, cost_val) in result.GetCosts() if "collision" in cost_type]
if len(collision_costs) > 0:
collision_err = np.asarray(collision_costs)
collision_costs = np.sum(collision_costs)
obj_value = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts()])
print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed")
print "{:>15} | {:>10}".format("COSTS", "-"*23)
print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost, joint_vel_cost2)
if np.isscalar(collision_costs):
print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)", collision_costs, "-")
print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs, pose_costs2)
print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-")
print ""
print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max")
print "{:>15} | {:>10}".format("ERRORS", "-"*23)
print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()), np.rad2deg(np.abs(joint_vel_err).max()))
if np.isscalar(collision_costs):
print "{:>15} | {:>10,.4} | {:>10,.4}".format("collision(s)", np.abs(-collision_err).min(), np.abs(-collision_err).max())
print "{:>15} | {:>10,.4} | {:>10,.4}".format("rot pose(s)", np.abs(pose_err[:,1:4]).min(), np.abs(pose_err[:,1:4]).max())
print "{:>15} | {:>10,.4} | {:>10,.4}".format("trans pose(s)", np.abs(pose_err[:,4:7]).min(), np.abs(pose_err[:,4:7]).max())
print ""
# make sure this function doesn't change state of the robot
assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices())
assert not np.any(orig_dof_vals - robot.GetDOFValues())
return traj, obj_value, pose_costs
def mat_to_base_pose_2D(mat):
pose = poseFromMatrix(mat)
x = pose[4]
y = pose[5]
return np.array([x, y])
def read_text_traj(filename,robot,dt=.01,scale=1.0):
""" Read in trajectory data stored in Youngbum's format (100Hz data):
HPY LHY LHR ... RWP (3-letter names)
+ - + ... + (sign of joint about equivalent global axis + / -)
0.0 5.0 2.0 ... -2.0 (Offset of joint from openHubo "zero" in YOUR sign
convention and scale)
1000 1000 pi/180 pi/180 ... pi/180 (scale of your units wrt openrave
default)
(data by row, single space separated)
"""
#TODO: handle multiple spaces
#Setup trajectory and source file
ind=_oh.make_name_to_index_converter(robot)
f=open(filename,'r')
#Read in header row to find joint names
header=f.readline().rstrip()
#print header.split(' ')
[traj,config]=create_trajectory(robot)
k=0
indices={}
Tindices={}
Tmap={'X':0,'Y':1,'Z':2,'R':3,'P':4,'W':5}
for s in header.split(' '):
j=ind(s)
if j>=0:
indices.setdefault(k,j)
try:
Tindices.setdefault(k,Tmap[s])
except KeyError:
pass
except:
raise
k=k+1
#Read in sign row
signlist=f.readline().rstrip().split(' ')
signs=[]
for s in signlist:
if s == '+':
signs.append(1)
else:
signs.append(-1)
#Read in offset row (fill with _np.zeros if not used)
offsetlist=f.readline().rstrip().split(' ')
offsets=[float(x) for x in offsetlist]
#Read in scale row (fill with _np.ones if not used)
scalelist=f.readline().rstrip().split(' ')
scales=[float(x) for x in scalelist]
pose=_oh.Pose(robot)
for line in f:
vals=[float(x) for x in line.rstrip().split(' ')]
Tdata=_np.zeros(6)
for i,v in enumerate(vals):
if indices.has_key(i):
pose[indices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
elif Tindices.has_key(i):
Tdata[Tindices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
#TODO: clip joint vals at limits
#TODO: use axis angle form for RPY data?
T=array(_comps.Transform.make_transform(Tdata[3:],Tdata[0:3]))
traj_append(traj,pose.to_waypt(dt,_rave.poseFromMatrix(T)))
return traj
def mat_to_base_pose(mat):
pose = poseFromMatrix(mat)
x = pose[4]
y = pose[5]
rot = axisAngleFromRotationMatrix(mat)[2]
return np.array([x, y, rot])
def plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj,
no_collision_cost_first=False, use_collision_cost=True, start_fixed=False, joint_vel_limits=None,
beta_pos=settings.BETA_POS, beta_rot=settings.BETA_ROT, gamma=settings.GAMMA, pose_weights=1, grasp_cup=False, R=0.02, D=0.035, cup_xyz=None):
orig_dof_inds = robot.GetActiveDOFIndices()
orig_dof_vals = robot.GetDOFValues()
n_steps = len(ee_trajs[0])
dof_inds = sim_util.dof_inds_from_name(robot, manip_name)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == len(dof_inds)
assert len(ee_link_names) == len(ee_trajs)
if no_collision_cost_first:
init_traj, _, _ = plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj,
no_collision_cost_first=False, use_collision_cost=False, start_fixed=start_fixed, joint_vel_limits=joint_vel_limits,
beta_pos = beta_pos, beta_rot = beta_rot, gamma = gamma)
else:
init_traj = old_traj.copy()
if start_fixed:
init_traj = np.r_[robot.GetDOFValues(dof_inds)[None,:], init_traj[1:]]
sim_util.unwrap_in_place(init_traj, dof_inds)
init_traj += robot.GetDOFValues(dof_inds) - init_traj[0,:]
request = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : manip_name,
"start_fixed" : start_fixed
},
"costs" : [
{
"type" : "joint_vel",
#"params": {"coeffs" : [gamma/(n_steps-1)]}
"params": {"coeffs" : [1]}
},
],
"constraints" : [
{
"type" : "cart_vel",
"name" : "cart_vel",
"params" : {
"max_displacement" : .02,
"first_step" : 0,
"last_step" : n_steps-1, #inclusive
"link" : "r_gripper_tool_frame"
}
},
#{
# "type" : "collision",
# "params" : {
# "continuous" : True,
# "coeffs" : [100]*(n_steps-3) + [0]*3,
# "dist_pen" : [0.01]
# }
#}
],
"init_info" : {
"type":"given_traj",
"data":[x.tolist() for x in init_traj]
}
}
if use_collision_cost:
request["costs"].append(
{
"type" : "collision",
"params" : {
"continuous" : True,
"coeffs" : [20000], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
#"coeffs" : [0] * (len(ee_trajs[0]) - 3) + [100]*3,
"dist_pen" : [0.025] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
}
})
if joint_vel_limits is not None:
request["constraints"].append(
{
"type" : "joint_vel_limits",
"params": {"vals" : joint_vel_limits,
"first_step" : 0,
"last_step" : n_steps-1
}
})
#coeff_mult = [0.0] * (len(poses) - 1) + [100]
#coeff_mult = np.linspace(1, 100, num=len(poses))
#coeff_mult_trans = np.logspace(-3, 1, num=len(ee_trajs[0]))
#coeff_mult_rot = np.logspace(-3, 1, num=len(ee_trajs[0]))
coeff_mult_trans = np.array([0] * (len(ee_trajs[0])-1) + [100])
coeff_mult_rot = np.array([0] * (len(ee_trajs[0])-1) + [100])
if not grasp_cup:
#coeff_mult_trans = np.array([0.1] * (len(ee_trajs[0])-1) + [100])
#coeff_mult_rot = np.array([1] * len(ee_trajs[0]))
for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj]
for (i_step,pose) in enumerate(poses):
if start_fixed and i_step == 0:
continue
request["costs"].append(
{"type":"pose",
"params":{
"xyz":pose[4:7].tolist(),
"wxyz":pose[0:4].tolist(),
"link":ee_link_name,
"timestep":i_step,
#"pos_coeffs":list(pose_weights * [np.sqrt(beta_pos/n_steps)]*3),
"pos_coeffs":[np.sqrt(beta_pos/n_steps) * coeff_mult_trans[i_step]]*3,
#"rot_coeffs":list(pose_weights * [np.sqrt(beta_rot/n_steps)]*3)
"rot_coeffs":[np.sqrt(beta_rot/n_steps) * coeff_mult_rot[i_step]]*3
}
})
print "Cup XYZ:", cup_xyz
s = json.dumps(request)
with openravepy.RobotStateSaver(robot):
with util.suppress_stdout():
prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem
# for t in range(1,n_steps):
# prob.AddCost(table_cost, [(t,j) for j in range(7)], "table%i"%t)
if grasp_cup:
tool_link = robot.GetLink("r_gripper_tool_frame")
local_dir = np.array([0.,0.,1.])
manip = robot.GetManipulator(manip_name)
arm_inds = manip.GetArmIndices()
arm_joints = [robot.GetJointFromDOFIndex(ind) for ind in arm_inds]
f = lambda x: gripper_tilt(x, robot, arm_inds, tool_link, local_dir)
f2 = lambda x: gripper_tilt2(x, robot, arm_inds, tool_link, local_dir)
dfdx = lambda x: gripper_tilt_dx(x, robot, arm_inds, tool_link, local_dir, arm_joints)
#for t in xrange(1,n_steps):
for t in xrange(n_steps-2,n_steps):
prob.AddConstraint(f, dfdx, [(t,j) for j in xrange(7)], "EQ", "up%i"%t)
#for t in xrange(1,n_steps-1):
# prob.AddConstraint(f2, [(t,j) for j in xrange(7)], "EQ", "up%i"%t)
#prob.AddConstraint(f2, [(n_steps-1,j) for j in xrange(7)], "EQ", "up%i"%(n_steps-1))
g = lambda x: gripper_around_cup(x, robot, arm_inds, tool_link, cup_xyz, R, D)
prob.AddConstraint(g, [(n_steps-1,j) for j in xrange(7)], "EQ", "cup%i"%(n_steps-1))
result = trajoptpy.OptimizeProblem(prob) # do optimization
print [(_name, viol) for (_name, viol) in result.GetConstraints() if viol > 0.0001]
#assert [viol <= 1e-4 for (_name,viol) in result.GetConstraints()]
traj = result.GetTraj()
pose_costs = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "pose"])
pose_err = []
with openravepy.RobotStateSaver(robot):
for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
ee_link = robot.GetLink(ee_link_name)
for (i_step, hmat) in enumerate(ee_traj):
if start_fixed and i_step == 0:
continue
robot.SetDOFValues(traj[i_step], dof_inds)
new_hmat = ee_link.GetTransform()
pose_err.append(openravepy.poseFromMatrix(mu.invertHmat(hmat).dot(new_hmat)))
pose_err = np.asarray(pose_err)
pose_costs2 = (beta_rot/n_steps) * np.square(pose_err[:,1:4]).sum() + (beta_pos/n_steps) * np.square(pose_err[:,4:7]).sum()
pose_costs3 = (beta_rot/n_steps) * np.dot(coeff_mult_rot[np.newaxis,1:], np.square(pose_err[:,1:4])).sum() + (beta_pos/n_steps) * np.dot(coeff_mult_trans[np.newaxis,1:], np.square(pose_err[:,4:7])).sum()
joint_vel_cost = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "joint_vel"])
joint_vel_err = np.diff(traj, axis=0)
joint_vel_cost2 = (gamma/(n_steps-1)) * np.square(joint_vel_err).sum()
sim_util.unwrap_in_place(traj, dof_inds)
joint_vel_err = np.diff(traj, axis=0)
collision_costs = [cost_val for (cost_type, cost_val) in result.GetCosts() if "collision" in cost_type]
#collision_costs = [cost_val for (cost_type, cost_val) in result.GetConstraints() if "collision" in cost_type]
if len(collision_costs) > 0:
collision_err = np.asarray(collision_costs)
collision_costs = np.sum(collision_costs)
obj_value = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts()])
print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed")
print "{:>15} | {:>10}".format("COSTS", "-"*23)
print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost, joint_vel_cost2)
if np.isscalar(collision_costs):
print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)", collision_costs, "-")
print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs, pose_costs2)
print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-")
print ""
print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max")
print "{:>15} | {:>10}".format("ERRORS", "-"*23)
print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()), np.rad2deg(np.abs(joint_vel_err).max()))
if np.isscalar(collision_costs):
print "{:>15} | {:>10,.4} | {:>10,.4}".format("collision(s)", np.abs(-collision_err).min(), np.abs(-collision_err).max())
print "{:>15} | {:>10,.4} | {:>10,.4}".format("rot pose(s)", np.abs(pose_err[:,1:4]).min(), np.abs(pose_err[:,1:4]).max())
print "{:>15} | {:>10,.4} | {:>10,.4}".format("trans pose(s)", np.abs(pose_err[:,4:7]).min(), np.abs(pose_err[:,4:7]).max())
print ""
# make sure this function doesn't change state of the robot
assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices())
assert not np.any(orig_dof_vals - robot.GetDOFValues())
rot_err = np.abs(pose_err[:,1:4]).sum()
pos_err = np.abs(pose_err[:,4:7]).sum()
print collision_err
return traj, obj_value, (pose_costs3, np.abs(collision_err[:-1]).sum())
}
# END pose_constraint
],
# BEGIN init
"init_info" : {
"type" : "straight_line", # straight line in joint space.
"endpoint" : init_joint_target.tolist() # need to convert numpy array to list
}
# END init
}
s = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(s, env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
print result
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
assert traj_is_safe(result.GetTraj(), robot) # Check that trajectory is collision free
# Now we'll check to see that the final constraint was satisfied
robot.SetActiveDOFValues(result.GetTraj()[-1])
posevec = openravepy.poseFromMatrix(robot.GetLink("r_gripper_tool_frame").GetTransform())
quat, xyz = posevec[0:4], posevec[4:7]
quat *= np.sign(quat.dot(quat_target))
if args.position_only:
assert (quat - quat_target).max() > 1e-3
else:
assert (quat - quat_target).max() < 1e-3
robot.SetDOFValues([rgj.GetLimits()[-1]], [rgj.GetDOFIndex()])
tlj = robot.GetJoint("torso_lift_joint")
robot.SetDOFValues([tlj.GetLimits()[-1]], [tlj.GetDOFIndex()])
print "processing point cloud"
if env.GetKinBody("convexsoup") is None:
cloud = cloudprocpy.readPCDXYZ("/home/joschu/Proj/trajoptrave/bigdata/laser_cloud.pcd")
cloud = cloudprocpy.boxFilter(cloud, -1,5,-5,5,.1,2)
aabb = robot.GetLink("base_link").ComputeAABB()
(xmin,ymin,zmin) = aabb.pos() - aabb.extents()
(xmax,ymax,zmax) = aabb.pos() + aabb.extents()
cloud = cloudprocpy.boxFilterNegative(cloud, xmin,xmax,ymin,ymax,zmin,zmax)
cloud = cloudprocpy.downsampleCloud(cloud, .015)
convex_soup.create_convex_soup(cloud, env)
print "done processing"
T = np.eye(4)
T[:3,3] = [ 1.27 , -0.2 , 0.790675]
##################
pose = rave.poseFromMatrix(T)
request = drive_to_reach_request(robot, "r_gripper_tool_frame", pose[4:7], pose[0:4])
s = json.dumps(request)
print "REQUEST:",s
trajoptpy.SetInteractive(True);
prob = trajoptpy.ConstructProblem(s, env)
result = trajoptpy.OptimizeProblem(prob)
def read_text_traj(filename,robot,dt=.01,scale=1.0):
""" Read in trajectory data stored in Youngbum's format (100Hz data):
HPY LHY LHR ... RWP (3-letter names)
+ - + ... + (sign of joint about equivalent global axis + / -)
0.0 5.0 2.0 ... -2.0 (Offset of joint from openHubo "zero" in YOUR sign
convention and scale)
1000 1000 pi/180 pi/180 ... pi/180 (scale of your units wrt openrave
default)
(data by row, single space separated)
"""
#TODO: handle multiple spaces
#Setup trajectory and source file
ind=_oh.make_name_to_index_converter(robot)
f=open(filename,'r')
#Read in header row to find joint names
header=f.readline().rstrip()
#print header.split(' ')
[traj,config]=create_trajectory(robot)
k=0
indices={}
Tindices={}
Tmap={'X':0,'Y':1,'Z':2,'R':3,'P':4,'W':5}
for s in header.split(' '):
j=ind(s)
if j>=0:
indices.setdefault(k,j)
try:
Tindices.setdefault(k,Tmap[s])
except KeyError:
pass
except:
raise
k=k+1
#Read in sign row
signlist=f.readline().rstrip().split(' ')
signs=[]
for s in signlist:
if s == '+':
signs.append(1)
else:
signs.append(-1)
#Read in offset row (fill with _np.zeros if not used)
offsetlist=f.readline().rstrip().split(' ')
offsets=[float(x) for x in offsetlist]
#Read in scale row (fill with _np.ones if not used)
scalelist=f.readline().rstrip().split(' ')
scales=[float(x) for x in scalelist]
pose=_oh.Pose(robot)
for line in f:
vals=[float(x) for x in line.rstrip().split(' ')]
Tdata=_np.zeros(6)
for i,v in enumerate(vals):
if indices.has_key(i):
pose[indices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
elif Tindices.has_key(i):
Tdata[Tindices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
#TODO: clip joint vals at limits
#TODO: use axis angle form for RPY data?
T=array(_comps.Transform.make_transform(Tdata[3:],Tdata[0:3]))
traj_append(traj,pose.to_waypt(dt,_rave.poseFromMatrix(T)))
return traj
