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