#/usr/bin/env python from hpp.corbaserver.manipulation.robot import Robot from hpp.corbaserver.manipulation import newProblem, ProblemSolver, ConstraintGraph, Rule from hpp.gepetto.manipulation import ViewerFactory from hpp import Transform import CORBA, sys, numpy as np newProblem() Robot.packageName = "talos_data" Robot.urdfName = "talos" Robot.urdfSuffix = '_full' Robot.srdfSuffix= '' class Box (object): rootJointType = 'freeflyer' packageName = 'hpp_tutorial' meshPackageName = 'hpp_tutorial' urdfName = 'cup' urdfSuffix = "" srdfSuffix = "" handles = [ "box/top", "box/bottom" ] robot = Robot ('dev', 'talos', rootJointType = "freeflyer") robot. leftAnkle = "talos/leg_left_6_joint" robot.rightAnkle = "talos/leg_right_6_joint" robot.setJointBounds ("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5]) ps = ProblemSolver (robot) ps.setRandomSeed(123)
args = p.parse_args () if args.context != defaultContext: createContext (args.context) print("context=" + args.context) loadServerPlugin (args.context, "manipulation-corba.so") isSimulation = args.context == "simulation" footPlacement = not isSimulation comConstraint = not isSimulation constantWaistYaw = not isSimulation fixedArmWhenGrasping = not isSimulation client = CorbaClient(context=args.context) newProblem(client=client.manipulation, name=args.context) robot, ps, vf, table, objects = makeRobotProblemAndViewerFactory(client, rolling_table=True, rosParam=args.ros_param) if isSimulation: ps.setMaxIterProjection (1) q_neutral = robot.getCurrentConfig() # Set robot to neutral configuration before building constraint graph robot.setCurrentConfig(q_neutral) ps.setDefaultLineSearchType ("ErrorNormBased") ps.setMaxIterPathPlanning (40) # create locked joint for table