def get_random_pose(env, limits, manip, frame_name="r_gripper_tool_frame"):
robot = env.GetRobots()[0]
is_success = False
while is_success is not True:
#Select which input space
index = np.random.choice(limits.keys())
x, y, z = generate_random_xyz(limits[index])
#simple check for collision with a box
if check_col_with_box(env, x, y, z):
continue
#do IK here
xyz = [x, y, z]
quat = [1, 0, 0, 0] # wxyz
pose = openravepy.matrixFromPose(np.r_[quat, xyz])
joint_angle = ku.ik_for_link(
pose,
manip,
frame_name,
filter_options=openravepy.IkFilterOptions.CheckEnvCollisions)
if joint_angle is None: continue
#further check for collision
robot.SetDOFValues(joint_angle, manip.GetArmIndices())
if env.CheckCollision(robot) or robot.CheckSelfCollision(): continue
is_success = True
xyz = np.array([x, y, z])
return joint_angle, xyz
def test_plan_to_pose(xyz, xyzw, leftright, robot, initial_state = build_robot_state(), planner_id='', target_link="%s_gripper_tool_frame"):
manip = robot.GetManipulator(leftright + "arm")
base_pose = initial_state.multi_dof_joint_state.poses[0]
base_q = base_pose.orientation
base_p = base_pose.position
t = rave.matrixFromPose([base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z])
robot.SetTransform(t)
joint_solutions = ku.ik_for_link(rave.matrixFromPose(np.r_[xyzw[3], xyzw[:3], xyz]), manip, target_link%leftright[0], 1, True)
if len(joint_solutions) == 0:
print "pose is not reachable"
return None
m = build_joint_request(joint_solutions[0], leftright, robot, initial_state, planner_id=planner_id)
try:
response = get_motion_plan(m)
if args.pause_after_response:
raw_input("press enter to continue")
except rospy.ServiceException:
return dict(returned = False)
mpr = response.motion_plan_response
if mpr.error_code.val != 1:
print "Planner returned error code", mpr.error_code.val
return dict(returned = True, error_code = mpr.error_code.val, safe=False)
else:
traj = [list(jtp.positions) for jtp in mpr.trajectory.joint_trajectory.points]
return dict(returned = True, safe = not check_traj(traj, manip, 100), traj = traj, planning_time = mpr.planning_time.to_sec(), error_code = mpr.error_code.val)
def planViewTarget(self, viewCenter, samplePair, configStart, keepout,
maxCSpaceJump, mover, viewEffector, visualize):
'''Calls trajectory optimization with viewing constraints for a pair of reachable sample positions.
- Input viewCenter: Point targeted for viewing.
- Input samplePair: A pair of w-space positions critical to viewing the object.
- Input configStart: The current configuration of the arm.
- Input keepout: Distance the sensor should keep from the center of the hand (meters).
- Input maxCSpaceJump: Estimated c-space distance between waypoints in the trajectory (radians).
- Input mover: ArmMover object for computing IK and checking collisions.
- Input viewEffector: Name of the sensor link in OpenRAVE.
- Input cloud: Point cloud (nx3 array) to add as obstacle. Removes before function returns. Ignores if None.
- Input visualize: True to run the OpenRAVE visualization for the trajopt run.
- Returns traj: List of 7-DOF configurations according to OpenRAVE ordering.
- Returns essIdxs: Indices in the trajectory that correspond to beginning, viewing (x2), and ending points.
- Returns trajData: Metadata for running this algorithm such as time, cost, and trajectory quality.
'''
timeStart = time_module.time()
#import pdb; pdb.set_trace()
# solve ik the two samples samples
configsPair = []
for i in xrange(2):
T = motion_planner.GeneratePose(samplePair[i], viewCenter)
solutions = ku.ik_for_link(T, mover.manip, viewEffector, return_all_solns=True, filter_options= \
openravepy.IkFilterOptions.CheckEnvCollisions)
if len(solutions) == 0:
raise Exception(
"No IK solutions found for input sample point.")
configsPair.append(solutions)
# switch order of pair if 2nd sample is closer
if motion_planner.ClosestDistance([configStart], configsPair[1]) < \
motion_planner.ClosestDistance([configStart], configsPair[0]):
samplePair = (samplePair[1], samplePair[0])
configsPair = (configsPair[1], configsPair[0])
# create constraints
constraints = [None, None]
for i in xrange(2):
constraints[i] = ViewConstraint(viewCenter, keepout, mover,
viewEffector)
# run trajopt
traj, essIdxs, cost, inCollision, meetsPosition, meetsKeepout, meetsOrient = \
self.smoothTrajectory(configStart, configsPair, constraints, maxCSpaceJump, mover, viewEffector, visualize)
# return result
time = time_module.time() - timeStart
trajData = TrajectoryData(time, cost, inCollision, meetsPosition,
meetsKeepout, meetsOrient)
return traj, essIdxs, trajData
def ik(self, pose):
"""
:param pose: tfx.pose
:return list of joints
"""
assert pose.frame.count('base_link') == 1
self.sim.update()
joints = ku.ik_for_link(np.array(pose.matrix), self.manip, self.tool_frame, 0)
return joints
def ik(self, pose):
"""
:param pose: tfx.pose
:return list of joints
"""
assert pose.frame.count('base_link') == 1
self.sim.update()
joints = ku.ik_for_link(np.array(pose.matrix), self.manip, self.tool_frame, 0)
return joints
def calcIKForTGivenLink(
self,
T,
linkName,
opts=openravepy.IkFilterOptions.CheckEnvCollisions):
'''TODO'''
return ku.ik_for_link(T,
self.manip,
linkName,
return_all_solns=True,
filter_options=opts)
def robot_state_from_pose_goal(xyz, xyzw, arm, robot, initial_state = build_robot_state()):
manip = robot.GetManipulator(arm + "arm")
joint_solutions = ku.ik_for_link(rave.matrixFromPose(np.r_[xyzw[3], xyzw[:3], xyz]),
manip, "%s_wrist_roll_link"%arm[0], 1, True)
if len(joint_solutions) == 0:
raise Exception
joints = robot.GetJoints()
joint_inds = robot.GetManipulator("%sarm"%arm).GetArmIndices()
joint_names = [joints[joint_inds[i]].GetName() for i in xrange(len(joint_solutions[0]))]
joint_values = [joint_solutions[0][i] for i in xrange(len(joint_solutions[0]))]
new_state = alter_robot_state(initial_state, joint_names, joint_values)
return new_state
def GetIKSolutions(env,
Tgoal,
manip=None,
getAll=True): #manip is OpenRAVE manip
sol = None
if (env == None or robot == None or manip == None):
print('\n Error: Undefined env, robot, or manip \n')
return sol
with env:
#sol = manip.FindIKSolutions(Tgoal, IkFilterOptions.CheckEnvCollisions) # get collision-free solution
if (getAll):
sol = ku.ik_for_link(
Tgoal,
manip,
manip.GetEndEffector().GetName(),
filter_options=openravepy.IkFilterOptions.CheckEnvCollisions,
return_all_solns=True)
else:
sol = manip.FindIKSolution(Tgoal,
IkFilterOptions.CheckEnvCollisions)
return sol
# cc = trajoptpy.GetCollisionChecker(env)
# root = ET.parse("/opt/ros/groovy/share/pr2_moveit_config/config/pr2.srdf").getroot()
# disabled_elems=root.findall("disable_collisions")
# for elem in disabled_elems:
# linki = robot.GetLink(elem.get("link1"))
# linkj = robot.GetLink(elem.get("link2"))
# if linki and linkj:
# cc.ExcludeCollisionPair(linki, linkj)
xs, ys, zs = np.mgrid[.35:.85:.05, 0:.5:.05, .8:.9:.1]
wxyz = [1, 0, 0, 0]
results = []
for (x, y, z) in zip(xs.flat, ys.flat, zs.flat):
wxyzxyz = np.r_[wxyz, x, y, z]
joint_solutions = ku.ik_for_link(openravepy.matrixFromPose(wxyzxyz),
robot.GetManipulator(manip),
"%s_gripper_tool_frame" % manip[0], 1,
True)
if len(joint_solutions) > 0:
target_joints = joint_solutions[0]
results.append(
plan(robot, manip, target_joints,
wxyzxyz if args.pose_goal else None))
success_frac = np.mean([result.success for result in results])
print "success frac:", success_frac
print "avg total time:", np.mean([result.t_total for result in results])
print "avg optimization time:", np.mean([result.t_opt for result in results])
print "avg verification time:", np.mean(
[result.t_verify for result in results])
robot = env.GetRobots()[0]
manip = robot.GetManipulator("rightarm")
robot.SetActiveManipulator(manip)
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
xyz_target = [.6, -.6, 1]
n_steps = 15
hmat_target = openravepy.matrixFromPose(np.r_[(0, 1, 0, 0), xyz_target])
init_joint_target = ku.ik_for_link(
hmat_target,
manip,
"r_gripper_tool_frame",
filter_options=openravepy.IkFilterOptions.CheckEnvCollisions)
request = {
"basic_info": {
"n_steps": n_steps,
"manip": "rightarm",
"start_fixed": True
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [1]
}
}, {
new_head_transform[0, 3] += 0.2
new_head_transform[2, 3] -= 0.1
baxter = env.GetRobots()[1]
b_manip = baxter.SetActiveManipulator("rightarm")
b_sol = []
goggles_trans = np.array(
[[-6.12323400e-17, -7.49879891e-33, 1.00000000e+00, 1.15732000e+00],
[1.22464680e-16, -1.00000000e+00, 0.00000000e+00, 0.00000000e+00],
[1.00000000e+00, 1.22464680e-16, 6.12323400e-17, 7.32410000e-01],
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
with env:
b_sol = ku.ik_for_link(
goggles_trans,
b_manip,
b_manip.GetEndEffector().GetName(),
filter_options=IkFilterOptions.IgnoreEndEffectorCollisions,
return_all_solns=True)
if (len(b_sol) > 0):
baxter.SetActiveDOFs(b_manip.GetArmIndices())
baxter.SetActiveDOFValues(b_sol[0])
# sol = []
# with env:
# sol = ku.ik_for_link(new_head_transform, manip, manip.GetEndEffector().GetName(),
# filter_options = IkFilterOptions.IgnoreSelfCollisions | IkFilterOptions.CheckEnvCollisions,
# return_all_solns = True)
# print "solutions",sol
# for s in sol:
# with env:
# robot.SetActiveDOFs(manip.GetArmIndices())
print "Joint start: " + str(joint_start)
robot.SetDOFValues(joint_start) # robot.GetManipulator('rightarm').GetArmIndices())
time.sleep(3)
roll = 0.0
pitch = 0.0
yaw = 0
quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
quat_target = [quat[3], quat[0], quat[1], quat[2]] # wxyz
xyz_target = [0.5, -0.2, 0.5]
hmat_target = openravepy.matrixFromPose( np.r_[quat_target, xyz_target] )
# BEGIN ik
init_joint_target = None
manip = robot.GetManipulator("rightarm")
init_joint_target = ku.ik_for_link(hmat_target, manip, "r_gripper_tool_frame",
filter_options = openravepy.IkFilterOptions.CheckEnvCollisions)
tries = 0
while ((init_joint_target == None) and (tries < 100)):
tries = tries + 1
xyz_target[0] = xyz_target[0] + random.uniform(-0.05, 0.05)
xyz_target[1] = xyz_target[1] + random.uniform(-0.05, 0.05)
xyz_target[2] = xyz_target[2] + random.uniform(-0.05, 0.05)
init_joint_target = ku.ik_for_link(hmat_target, manip, "r_gripper_tool_frame",
filter_options = openravepy.IkFilterOptions.CheckEnvCollisions)
print "Final target: " + str(xyz_target) + " Took " + str(tries) + " tries."
# END ik
print "Target: " + str(init_joint_target)
robot.SetDOFValues(init_joint_target, robot.GetManipulator('rightarm').GetArmIndices())
time.sleep(3)
# import xml.etree.ElementTree as ET
# cc = trajoptpy.GetCollisionChecker(env)
# root = ET.parse("/opt/ros/groovy/share/pr2_moveit_config/config/pr2.srdf").getroot()
# disabled_elems=root.findall("disable_collisions")
# for elem in disabled_elems:
# linki = robot.GetLink(elem.get("link1"))
# linkj = robot.GetLink(elem.get("link2"))
# if linki and linkj:
# cc.ExcludeCollisionPair(linki, linkj)
xs, ys, zs = np.mgrid[.35:.85:.05, 0:.5:.05, .8:.9:.1]
wxyz = [1,0,0,0]
results = []
for (x,y,z) in zip(xs.flat, ys.flat, zs.flat):
wxyzxyz = np.r_[wxyz, x,y,z]
joint_solutions = ku.ik_for_link(openravepy.matrixFromPose(wxyzxyz), robot.GetManipulator(manip), "%s_gripper_tool_frame"%manip[0], 1, True)
if len(joint_solutions) > 0:
target_joints = joint_solutions[0]
results.append( plan(robot, manip, target_joints, wxyzxyz if args.pose_goal else None) )
success_frac = np.mean([result.success for result in results])
print "success frac:", success_frac
print "avg total time:", np.mean([result.t_total for result in results])
print "avg optimization time:", np.mean([result.t_opt for result in results])
print "avg verification time:", np.mean([result.t_verify for result in results])
robot.SetDOFValues([GRIPPER_OPEN_CODE], manip.GetGripperIndices())
ikmodel = databases.inversekinematics.InverseKinematicsModel(
robot, iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
ikmodel.setrobot()
vec_target = [0.125, -0.0274, 0.990, 0.052, -0.078, -0.898, 0.263]
quat_target, xyz_target = vec_target[:4], vec_target[4:]
mat_target = matrixFromPose(vec_target)
manip = robot.GetManipulator('grip')
init_joint_target = ku.ik_for_link(
mat_target,
manip,
"Palm",
filter_options=IkFilterOptions.CheckEnvCollisions)
request = {
"basic_info": {
"n_steps": 10,
"manip": "grip",
"start_fixed": True
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [1]
}
}, {
"type": "collision",
def goToArmPose(self, pose, isPlanned, reqName=Constants.Request.noRequest):
"""
Go to PoseStamped pose
If isPlanned is True, then trajopt is used to plan the trajectory
Otherwise, just IK is used to plan trajectory
If IK fails, arm doesn't move
The methods for executing the trajectories are in pr2/pr2.py
"""
# must convert to BaseLink frame
if self.listener != None:
try:
commonTime = self.listener.getLatestCommonTime(Constants.BaseLink,pose.header.frame_id)
pose.header.stamp = commonTime
pose = self.listener.transformPose(Constants.BaseLink,pose)
except tf.Exception:
return
if pose.header.frame_id != Constants.BaseLink:
return
# get the current joint state for joint_names
rospy.wait_for_service("return_joint_states")
s = rospy.ServiceProxy("return_joint_states", ReturnJointStates)
resp = s(self.joint_names)
# set the start joint position
joint_start = resp.position
self.robot.SetDOFValues(joint_start, self.robot.GetManipulator(self.armName).GetArmIndices())
# initialize trajopt inputs
quat = pose.pose.orientation
xyz = pose.pose.position
quat_target = [quat.w, quat.x, quat.y, quat.z]
xyz_target = [xyz.x, xyz.y, xyz.z]
hmat_target = openravepy.matrixFromPose( np.r_[quat_target, xyz_target] )
# if no planning
if not isPlanned:
self.arm.goto_pose_matrix(hmat_target, Constants.BaseLink, self.toolframe)
return True
# inverse kinematics
manip = self.robot.GetManipulator(self.armName)
init_joint_target = ku.ik_for_link(hmat_target, manip, self.toolframe, filter_options = openravepy.IkFilterOptions.CheckEnvCollisions)
if init_joint_target == None:
# inverse kinematics failed
# will do nothing for now, may want to alter xyz_target a little
rospy.loginfo('IK failed')
return False
request = self.getRequest(reqName, xyz_target, quat_target, init_joint_target)
if request == None:
return
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.env)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
self.arm.follow_joint_trajectory(result.GetTraj())
return True
def grab_test(tb, robot, env, human, goggles, forehead):
h = human.GetTransform()
h[0, 3] -= 0.5
t = goggles.GetTransform()
t[0, 3] -= 0.5
f = forehead.GetTransform()
f[0, 3] -= 0.5
with env:
human.SetTransform(h)
goggles.SetTransform(t)
forehead.SetTransform(f)
t = goggles.GetTransform()
t[0, 3] -= 0.1
handles.append(
env.plot3(points=t[0:3, 3], pointsize=5.0, colors=array(((0, 0, 1)))))
rot_mat = np.array([[np.cos(np.pi), -np.sin(np.pi), 0],
[np.sin(np.pi), np.cos(np.pi), 0], [0, 0, 1]])
t[0:3, 0:3] = np.dot(t[0:3, 0:3], rot_mat)
direction = [0, 1, 0]
angle = np.pi * 0.5
rot_mat = transformations.rotation_matrix(angle, direction)
t[0:3, 0:3] = np.dot(rot_mat[0:3, 0:3], t[0:3, 0:3])
ikSolutions = ku.ik_for_link(
t,
tb.r_manip,
tb.r_manip.GetEndEffector().GetName(),
filter_options=openravepy.IkFilterOptions.CheckEnvCollisions,
return_all_solns=True)
from_vec = t[0:3, 3]
to_vec_1 = from_vec + 0.05 * (t[0:3, 0])
to_vec_2 = from_vec + 0.05 * (t[0:3, 1])
to_vec_3 = from_vec + 0.05 * (t[0:3, 2])
handles.append(env.drawarrow(from_vec, to_vec_1, 0.005, [1, 0, 0]))
handles.append(env.drawarrow(from_vec, to_vec_2, 0.005, [0, 1, 0]))
handles.append(env.drawarrow(from_vec, to_vec_3, 0.005, [0, 0, 1]))
raw_input("Press enter to move to ready...")
print len(ikSolutions)
with env:
robot.SetActiveManipulator(tb.r_manip)
robot.SetActiveDOFs(tb.r_manip.GetArmIndices())
robot.SetActiveDOFValues(ikSolutions[0])
raw_input("Press enter to grab...")
with env:
robot.Grab(env.GetKinBody('goggles'))
# # tb.TestInterp()
raw_input("Press enter to continue...")
tuckarms(env, robot)
raw_input("Press enter to continue...")
def goToArmPose(self, lr, pose):
body = rave.RaveCreateKinBody(self.env,'')
body.SetName('testbody')
body.InitFromBoxes(np.array([[0,0,0,20,20,pose.pose.position.z - .05]]),True) # set geometry as one box of extents 0.1, 0.2, 0.3
#self.env.Add(body,True)
print(pose.pose.position.z)
if lr == 'l':
arm = self.larm
armName = 'leftarm'
else:
arm = self.rarm
armName = 'rightarm'
joint_names = [lr+suffix for suffix in self.joint_name_suffixes]
toolframe = lr + self.tool_frame_suffix
rospy.wait_for_service("return_joint_states")
s = rospy.ServiceProxy("return_joint_states", ReturnJointStates)
resp = s(joint_names)
#print(pplist(resp.position))
joint_start = resp.position
self.robot.SetDOFValues(joint_start, self.robot.GetManipulator(armName).GetArmIndices())
quat = pose.pose.orientation
xyz = pose.pose.position
quat_target = [quat.w, quat.x, quat.y, quat.z]
xyz_target = [xyz.x, xyz.y, xyz.z]
elbow_target = [0, 0, xyz.z + .1]
#quat_target = [1,0,0,0] # wxyz
#xyz_target = [6.51073449e-01, -1.87673551e-01, 4.91061915e-01]
hmat_target = openravepy.matrixFromPose( np.r_[quat_target, xyz_target] )
# BEGIN ik
manip = self.robot.GetManipulator(armName)
init_joint_target = ku.ik_for_link(hmat_target, manip, toolframe, filter_options = openravepy.IkFilterOptions.CheckEnvCollisions)
# if init_joint_target is NONE, return failure. or try a nearby point
# END ik
n_steps = 10
request = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : armName,
"start_fixed" : True
},
"costs" : [
{
"type" : "joint_vel",
"params": {"coeffs" : [1]}
},
{
"type" : "collision",
"params" : {
"coeffs" : [20],
"dist_pen" : [0.025]
}
},
{
"type" : "pose",
"params" : {
"coeffs" : [20],
"xyz" : elbow_target,
"wxyz" : quat_target,
"link": "l_elbow_flex_link",
"rot_coeffs" : [0,0,0],
"pos_coeffs" : [0,0,1]
}
},
],
"constraints" : [
# BEGIN pose_target
{
"type" : "pose",
"name" : "target_pose",
"params" : {"xyz" : xyz_target,
"wxyz" : quat_target, # unused
"link": toolframe,
"rot_coeffs" : [1,1,0],
"pos_coeffs" : [1,1,1]
}
},
#END pose_target
],
"init_info" : {
"type" : "straight_line",
"endpoint" : init_joint_target.tolist()
}
}
"""
request = {
"basic_info" : {
"n_steps" : 10,
"manip" : armName, # see below for valid values
"start_fixed" : True # i.e., DOF values at first timestep are fixed based on current robot state
},
"costs" : [
{
"type" : "joint_vel", # joint-space velocity cost
"params": {"coeffs" : [1]} # a list of length one is automatically expanded to a list of length n_dofs
# Also valid: "coeffs" : [7,6,5,4,3,2,1]
},
{
"type" : "continuous_collision",
"name" :"cont_coll", # shorten name so printed table will be prettier
"params" : {
"coeffs" : [20], # 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
}
}
],
"constraints" : [
# BEGIN pose_constraint
{
"type" : "pose",
"params" : {"xyz" : xyz_target,
"wxyz" : quat_target,
"link": toolframe,
# "timestep" : 9
# omitted because timestep = n_steps-1 is default
# "pos_coeffs" : [1,1,1], # omitted because that's default
"pos_coeffs" : [1,1,1],
"rot_coeffs" : [1,1,1]
}
} ,
# 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, self.env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
#print(result.GetTraj())
arm.follow_joint_trajectory(result.GetTraj())
joint_start = resp.position
print "Joint start: " + str(joint_start)
robot.SetDOFValues(joint_start) # robot.GetManipulator('rightarm').GetArmIndices())
#time.sleep(3)
quat_target = [req.goal.pose.orientation.w, req.goal.pose.orientation.x, req.goal.pose.orientation.y, req.goal.pose.orientation.z] # wxyz
xyz_target = [req.goal.pose.position.x, req.goal.pose.position.y, req.goal.pose.position.z]
hmat_target = openravepy.matrixFromPose( np.r_[quat_target, xyz_target] )
# BEGIN ik
init_joint_target = None
manip = robot.GetManipulator("rightarm")
init_joint_target = ku.ik_for_link(hmat_target, manip, "r_gripper_tool_frame")
tries = 0
"""
while ((init_joint_target == None) and (tries < 100)):
tries = tries + 1
xyz_target[0] = xyz_target[0] + random.uniform(-0.05, 0.05)
xyz_target[1] = xyz_target[1] + random.uniform(-0.05, 0.05)
xyz_target[2] = xyz_target[2] + random.uniform(-0.05, 0.05)
init_joint_target = ku.ik_for_link(hmat_target, manip, "r_gripper_tool_frame",
filter_options = openravepy.IkFilterOptions.CheckEnvCollisions)
"""
print "Final target: " + str(xyz_target) + " Took " + str(tries) + " tries."
# END ik
print "Target: " + str(init_joint_target)
def goToArmPose(self,
pose,
isPlanned,
reqName=Constants.Request.noRequest):
"""
Go to PoseStamped pose
If isPlanned is True, then trajopt is used to plan the trajectory
Otherwise, just IK is used to plan trajectory
If IK fails, arm doesn't move
The methods for executing the trajectories are in pr2/pr2.py
"""
# must convert to BaseLink frame
if self.listener != None:
try:
commonTime = self.listener.getLatestCommonTime(
Constants.BaseLink, pose.header.frame_id)
pose.header.stamp = commonTime
pose = self.listener.transformPose(Constants.BaseLink, pose)
except tf.Exception:
return
if pose.header.frame_id != Constants.BaseLink:
return
# get the current joint state for joint_names
rospy.wait_for_service("return_joint_states")
s = rospy.ServiceProxy("return_joint_states", ReturnJointStates)
resp = s(self.joint_names)
# set the start joint position
joint_start = resp.position
self.robot.SetDOFValues(
joint_start,
self.robot.GetManipulator(self.armName).GetArmIndices())
# initialize trajopt inputs
quat = pose.pose.orientation
xyz = pose.pose.position
quat_target = [quat.w, quat.x, quat.y, quat.z]
xyz_target = [xyz.x, xyz.y, xyz.z]
hmat_target = openravepy.matrixFromPose(np.r_[quat_target, xyz_target])
# if no planning
if not isPlanned:
self.arm.goto_pose_matrix(hmat_target, Constants.BaseLink,
self.toolframe)
return True
# inverse kinematics
manip = self.robot.GetManipulator(self.armName)
init_joint_target = ku.ik_for_link(
hmat_target,
manip,
self.toolframe,
filter_options=openravepy.IkFilterOptions.CheckEnvCollisions)
if init_joint_target == None:
# inverse kinematics failed
# will do nothing for now, may want to alter xyz_target a little
rospy.loginfo('IK failed')
return False
request = self.getRequest(reqName, xyz_target, quat_target,
init_joint_target)
if request == None:
return
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.env)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
self.arm.follow_joint_trajectory(result.GetTraj())
return True
def ikfunc(hmat):
return ku.ik_for_link(hmat, manip, link_name, return_all_solns=True)
robot.SetDOFValues(joint_start, manip.GetArmIndices())
robot.SetDOFValues([GRIPPER_OPEN_CODE], manip.GetGripperIndices())
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
ikmodel.setrobot()
vec_target = [0.125, -0.0274, 0.990, 0.052, -0.078, -0.898, 0.263]
quat_target, xyz_target = vec_target[:4], vec_target[4:]
mat_target = matrixFromPose(vec_target)
manip = robot.GetManipulator('grip')
init_joint_target = ku.ik_for_link(mat_target, manip, "Palm",
filter_options = IkFilterOptions.CheckEnvCollisions)
request = {
"basic_info" : {
"n_steps" : 10,
"manip" : "grip",
"start_fixed" : True
},
"costs" : [
{
"type" : "joint_vel",
"params": {"coeffs" : [1]}
},
{
"type" : "collision",
"name" : "cont_coll",
"params" : {
