robot = env.GetRobots()[0]
ikmodel=databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.TranslationXY2D)
if not ikmodel.load():
ikmodel.autogenerate()
print('IK model autogenerated!')
T_gripper = robot.GetLink("Finger").GetTransform()
T_grasp = T_gripper.copy()
T_grasp[:3,3] += np.array([-0.5,0.2,0])
T_grasp = T_grasp.dot(matrixFromAxisAngle([0,0,1],np.pi/2))
xyz_targ = T_grasp[:3,3]
quat_targ = quatFromRotationMatrix(T_grasp[:3,:3])
mk.create_mesh_box(env, np.array([0.2,0.25,0]), np.array([0.05,0.05,0.3]), "box1")
target=T_grasp[:2,3]
print(target)
h=env.plot3(np.array([target[0],target[1],ikmodel.manip.GetTransform()[2,3]]),10.0)
robot.SetDOFLimits([-10, -10, -10],[10,10,10]) # to restrict space of IK solutions
robot.SetActiveDOFs(robot.SetActiveManipulator('arm').GetArmIndices())
ikp=IkParameterization(target,IkParameterization.Type.TranslationXY2D)
manip=interfaces.BaseManipulation(robot)
#traj = manip.MoveToHandPosition(ikparam=ikp,seedik=10,execute=False,outputtrajobj=True)
#print(traj)
env.StopSimulation()
env.Load(ENV_FILE)
robot = env.GetRobots()[0]
robot.SetDOFValues([ 0 ,0 ,0]);
manip = robot.GetManipulator(MANIP_NAME)
viewer = trajoptpy.GetViewer(env)
viewer.SetCameraTransformation([0,0,20], [0,0,0], [0,1,0])
##################
T_gripper = np.eye(4)
T_gripper[:3,3] = np.array([2,2,0])
robot.GetLink(LINK_NAME).SetTransform(T_gripper)
T_grasp = np.eye(4)
xyz_targ = T_grasp[:3,3]
# mk.create_mesh_box(env, np.array([5+0.01,0,0]), np.array([0.01,10,0.01]))
mk.create_mesh_box(env, np.array([3.5,3,0]), np.array([1,1,2]), "box1")
mk.create_mesh_box(env, np.array([3.5,-0.5,0]), np.array([1,1,2]), "box2")
# mk.create_mesh_box(env, np.array([3.5,1.25,0]), np.array([0.2,0.2,1]), "box3")
# mk.create_mesh_box(env, np.array([3.5,2,0]), np.array([0.2,0.2,1]), "box4")
quat_targ = rave.quatFromRotationMatrix(T_grasp[:3,:3])
request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME)
##################
'''
# first optimize ignoring collision costs
all_costs = request["costs"]
noncollision_costs = [cost for cost in request["costs"] if "collision" not in cost["type"]]
request["costs"] = noncollision_costs
