Python Robot Examples, hpp.corbaserver.rbprm.talos.Robot Python Examples

Example #1

0

Show file

from numpy.linalg import norm
import random
from math import isnan
import sl1m.planner_scenarios.talos.slalom_debris as lp
tp = lp.tp
pb, coms, footpos, allfeetpos, res = lp.solve() 


print("Done.")
import time
Robot.urdfSuffix+="_safeFeet"

DEFAULT_COM_HEIGHT = 0.86
Z_AXIS = lp.Z_AXIS
pId = tp.ps.numberPaths() -1
fullBody = Robot ()

# Set the bounds for the root
fullBody.setJointBounds ("root_joint", [-20,20 ,-20, 20, 0, 2.8])
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.client.robot.setExtraConfigSpaceBounds(tp.extraDofBounds)
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
ps = tp.ProblemSolver( fullBody )

#load the viewer
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)

# load a reference configuration
q_ref = fullBody.referenceConfig_elbowsUp[::] +[0]*6
fullBody.setCurrentConfig (q_ref)
fullBody.setReferenceConfig(q_ref)

Example #2

0

Show file

File: talos_navBauzil.py Project: pFernbach/hpp-rbprm-corba

from hpp.corbaserver.rbprm.talos import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print "Plan guide trajectory ..."
import talos_navBauzil_path as tp
print "Done."
import time

pId = tp.ps.numberPaths() - 1
fullBody = Robot()
# Set the bounds for the root, take slightly larger bounding box than during root planning
root_bounds = tp.root_bounds[::]
root_bounds[0] -= 0.2
root_bounds[1] += 0.2
root_bounds[2] -= 0.2
root_bounds[3] += 0.2
root_bounds[-1] = 1.2
root_bounds[-2] = 0.8
fullBody.setJointBounds("root_joint", root_bounds)
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([
    -tp.vMax, tp.vMax, -tp.vMax, tp.vMax, 0, 0, -tp.aMax, tp.aMax, -tp.aMax,
    tp.aMax, 0, 0
])
ps = tp.ProblemSolver(fullBody)
ps.setParameter("Kinodynamic/velocityBound", tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound", tp.aMax)
#load the viewer
v = tp.Viewer(ps, viewerClient=tp.v.client, displayCoM=True)

Example #3

0

Show file

from hpp.corbaserver.rbprm.talos import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
from hpp.gepetto import ViewerFactory
from hpp.corbaserver import ProblemSolver
import os
import random
import time
statusFilename = "/res/infos.log"

fullBody = Robot()
# Set the bounds for the root
fullBody.setJointBounds("root_joint", [-0.3, 0.3, -0.3, 0.3, 0.8, 1.05])
## reduce bounds on joints along x, to put conservative condition on the contact generation for sideway steps
joint6L_bounds_prev = fullBody.getJointBounds('leg_left_6_joint')
joint2L_bounds_prev = fullBody.getJointBounds('leg_left_2_joint')
joint6R_bounds_prev = fullBody.getJointBounds('leg_right_6_joint')
joint2R_bounds_prev = fullBody.getJointBounds('leg_right_2_joint')
fullBody.setJointBounds('leg_left_6_joint', [-0.25, 0.25])
fullBody.setJointBounds('leg_left_2_joint', [-0.25, 0.25])
fullBody.setJointBounds('leg_right_6_joint', [-0.25, 0.25])
fullBody.setJointBounds('leg_right_2_joint', [-0.25, 0.25])
# constraint z axis and y axis :
joint1L_bounds_prev = fullBody.getJointBounds('leg_left_1_joint')
joint3L_bounds_prev = fullBody.getJointBounds('leg_left_3_joint')
joint1R_bounds_prev = fullBody.getJointBounds('leg_right_1_joint')
joint3R_bounds_prev = fullBody.getJointBounds('leg_right_3_joint')
fullBody.setJointBounds('leg_left_1_joint', [-0.2, 0.7])
fullBody.setJointBounds('leg_left_3_joint', [-1.3, 0.4])
fullBody.setJointBounds('leg_right_1_joint', [-0.7, 0.2])
fullBody.setJointBounds('leg_right_3_joint', [-1.3, 0.4])

Example #4

0

Show file

File: talos_table.py Project: teguhSL/hpp-rbprm-corba

from hpp.corbaserver.rbprm.talos import Robot
from hpp.gepetto import Viewer
import time
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
import time


fullBody = Robot ()
fullBody.setJointBounds ("root_joint",  [-5,5, -1.5, 1.5, 0.95, 1.05])
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.client.robot.setExtraConfigSpaceBounds([0]*12)
ps = ProblemSolver( fullBody )


from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
vf.loadObstacleModel ("hpp_environments", "multicontact/table_140_70_73", "planning")



q_ref = [
        0.0, 0.0,  1.0232773,  0.0 ,  0.0, 0.0, 1,                   #Free flyer
        0.0,  0.0, -0.411354,  0.859395, -0.448041, -0.001708,          #Left Leg
        0.0,  0.0, -0.411354,  0.859395, -0.448041, -0.001708,          #Right Leg
        0.0 ,  0.006761,                                                #Chest
        0.25847 ,  0.173046, -0.0002, -0.525366, 0.0, -0.0,  0.1,-0.005,  #Left Arm
        -0.25847 , -0.173046, 0.0002  , -0.525366, 0.0,  0.0,  0.1,-0.005,#Right Arm
        0.,  0.,
0,0,0,0,0,0]; # head

Example #5

0

Show file

#Robot.urdfSuffix += "_safeFeet"
statusFilename = tp.statusFilename
pId = 0
f = open(statusFilename, "a")
if tp.ps.numberPaths() > 0:
    print "Path planning OK."
    f.write("Planning_success: True" + "\n")
    f.close()
else:
    print "Error during path planning"
    f.write("Planning_success: False" + "\n")
    f.close()
    import sys
    sys.exit(1)

fullBody = Robot()

# Set the bounds for the root
rootBounds = tp.rootBounds[::]
rootBounds[-2] -= 0.2
rootBounds[0] -= 0.2
rootBounds[1] += 0.2
rootBounds[2] -= 0.2
rootBounds[3] += 0.2
fullBody.setJointBounds("root_joint", rootBounds)
fullBody.setConstrainedJointsBounds()

# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([
    -tp.vMax, tp.vMax, -tp.vMax, tp.vMax, 0, 0, -tp.aMax, tp.aMax, -tp.aMax,

Example #6

0

Show file

File: talos_platform.py Project: teguhSL/hpp-rbprm-corba

statusFilename = tp.statusFilename

f = open(statusFilename,"a")
if tp.ps.numberPaths() > 0 :
  print "Path planning OK."
  f.write("Planning_success: True"+"\n")
  f.close()
else :
  print "Error during path planning"
  f.write("Planning_success: False"+"\n")
  f.close()
  import sys
  sys.exit(1)
"""

fullBody = Robot()

# Set the bounds for the root
fullBody.setJointBounds("root_joint", tp.rootBounds)
fullBody.setConstrainedJointsBounds()

# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([
    -tp.vMax, tp.vMax, -tp.vMax, tp.vMax, 0, 0, -tp.aMax, tp.aMax, -tp.aMax,
    tp.aMax, 0, 0
])
ps = tp.ProblemSolver(fullBody)
ps.setParameter("Kinodynamic/velocityBound", tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound", tp.aMax)
#load the viewer

Example #7

0

Show file

import time
statusFilename = tp.statusFilename
pId = 0
f = open(statusFilename, "a")
if tp.ps.numberPaths() > 0:
    print "Path planning OK."
    f.write("Planning_success: True" + "\n")
    f.close()
else:
    print "Error during path planning"
    f.write("Planning_success: False" + "\n")
    f.close()
    import sys
    sys.exit(1)

fullBody = Robot()

# Set the bounds for the root
fullBody.setJointBounds("root_joint", [-2, 2, -2, 2, 0.9, 1.05])
## reduce bounds on joints along x, to put conservative condition on the contact generation for sideway steps
joint6L_bounds_prev = fullBody.getJointBounds('leg_left_6_joint')
joint2L_bounds_prev = fullBody.getJointBounds('leg_left_2_joint')
joint6R_bounds_prev = fullBody.getJointBounds('leg_right_6_joint')
joint2R_bounds_prev = fullBody.getJointBounds('leg_right_2_joint')
fullBody.setJointBounds('leg_left_6_joint', [-0.25, 0.25])
fullBody.setJointBounds('leg_left_2_joint', [-0.25, 0.25])
fullBody.setJointBounds('leg_right_6_joint', [-0.25, 0.25])
fullBody.setJointBounds('leg_right_2_joint', [-0.25, 0.25])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([

Example #8

0

Show file

File: talos_navBauzil_hard.py Project: teguhSL/hpp-rbprm-corba

from hpp.corbaserver.rbprm.talos import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time

print "Plan guide trajectory ..."
import talos_navBauzil_hard_path as tp

print "Done."
import time

Robot.urdfSuffix += "_safeFeet"

pId = tp.ps.numberPaths() - 1
fullBody = Robot()

# Set the bounds for the root, take slightly larger bounding box than during root planning
root_bounds = tp.root_bounds[::]
root_bounds[0] -= 0.2
root_bounds[1] += 0.2
root_bounds[2] -= 0.2
root_bounds[3] += 0.2
root_bounds[-1] = 1.2
root_bounds[-2] = 0.8
fullBody.setJointBounds("root_joint", root_bounds)
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([
    -tp.vMax, tp.vMax, -tp.vMax, tp.vMax, 0, 0, -tp.aMax, tp.aMax, -tp.aMax,
    tp.aMax, 0, 0
])

Example #9

0

Show file

File: talos_plateformes.py Project: teguhSL/hpp-rbprm-corba

from hpp.corbaserver.rbprm.talos import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print "Plan guide trajectory ..."
import talos_plateformes_path as tp
print "Done."
import time
Robot.urdfSuffix += "_safeFeet"
pId = tp.ps.numberPaths() - 1
fullBody = Robot()

# Set the bounds for the root
fullBody.setJointBounds("root_joint", [-5, 5, -1.5, 1.5, 0.95, 1.3])
fullBody.setConstrainedJointsBounds()
fullBody.setJointBounds('leg_left_1_joint', [-0.1, 0.2])
fullBody.setJointBounds('leg_right_1_joint', [-0.2, 0.1])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([
    -tp.vMax, tp.vMax, -tp.vMax, tp.vMax, 0, 0, -tp.aMax, tp.aMax, -tp.aMax,
    tp.aMax, 0, 0
])
ps = tp.ProblemSolver(fullBody)
ps.setParameter("Kinodynamic/velocityBound", tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound", tp.aMax)
#load the viewer
v = tp.Viewer(ps, viewerClient=tp.v.client, displayCoM=True)

# load a reference configuration
q_ref = fullBody.referenceConfig[::] + [0] * 6

Example #10

0

Show file

File: talos_flatGround.py Project: teguhSL/hpp-rbprm-corba

from hpp.corbaserver.rbprm.talos import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print "Plan guide trajectory ..."
import talos_flatGround_path as tp
print "Done."
import time

pId = tp.ps.numberPaths() - 1
fullBody = Robot()

# Set the bounds for the root
fullBody.setJointBounds("root_joint", [-5, 5, -1.5, 1.5, 0.95, 1.05])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([
    -tp.vMax, tp.vMax, -tp.vMax, tp.vMax, 0, 0, -tp.aMax, tp.aMax, -tp.aMax,
    tp.aMax, 0, 0
])
ps = tp.ProblemSolver(fullBody)
ps.setParameter("Kinodynamic/velocityBound", tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound", tp.aMax)
#load the viewer
v = tp.Viewer(ps, viewerClient=tp.v.client, displayCoM=True)

# load a reference configuration
q_ref = fullBody.referenceConfig[::] + [0] * 6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig(q_init)