#/usr/bin/env python
# Script which goes with animals_description package.
# Easy way to test planning algo (no internal DoF) on SO3 joint.
from hpp.corbaserver.ant_sphere import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import numpy as np
from viewer_display_library import normalizeDir, plotVerticalCone, plotCone, plotPath, plotVerticalConeWaypoints, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints
from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-2, 2, -2, 2, -0.1, 8])
ps = ProblemSolver (robot)
cl = robot.client
# Configs : [x, y, z, qw, qx, qy, qz, nx, ny, nz, theta]
q1 = [0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0]
q2 = [1, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0]
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("animals_description","cave","cave")
#cl.obstacle.loadObstacleModel('animals_description','cave','cave')
r(q2)
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
# verif orientation:
#/usr/bin/env python
# Script which goes with animals_description or room_description package.
# Easy way to test planning algo (no internal DoF) on SO3 joint.
## WARNING: DEPRECATED FILE !!!!!! See "ant_in_room.py" instead
#from hpp.corbaserver.ant import Robot
from hpp.corbaserver.ant_sphere import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import numpy as np
from viewer_display_library import normalizeDir, plotVerticalCone, plotCone, plotPath, plotVerticalConeWaypoints, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints
from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-5, 5, -5, 5, -0.01, 3]) # room
ps = ProblemSolver (robot)
cl = robot.client
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("room_description","room","room")
# Configs : [x, y, z, qw, qx, qy, qz, nx, ny, nz, theta]
#q11 = [0, -1, 0.002, 0, 0, 0, 0, 0, 1]; #q22 = [-0.1, 1.75, 0.302, 1.57, 0, 0, 0, 0, 1]
q11 = [1.781, 1.4, 1.3, 0, 0, 0, 1, 0, 0, 1]; #q22 = [0.8, -2.6, 2.35, 0, 0, 0, 1, 0, 0, 1]
#q22 = [0.45, 1.4, 0.8, 0, 0, 0, 1, 0, 0, 1] # easier
q22 = [0, 2, 0.23, 1, 0, 0, 0, 0, 0, 1] # under table