#/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