def load_fullbody(self):
from talos_rbprm.talos import Robot
# use a model with upscaled collision geometry for the feet
Robot.urdfSuffix += "_safeFeet"
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig_legsApart[::] + [0] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
def load_fullbody(self):
from talos_rbprm.talos import Robot
Robot.urdfSuffix += "_safeFeet"
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [
0
] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [
0
] * self.path_planner.extra_dof
def load_fullbody(self):
from talos_rbprm.talos import Robot
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [
0
] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [
0
] * self.path_planner.extra_dof
self.fullbody.limbs_names = [
self.fullbody.rLegId, self.fullbody.lLegId
]
class ContactGenerator(TalosContactGenerator):
def __init__(self):
super().__init__(PathPlanner())
def load_fullbody(self):
from talos_rbprm.talos import Robot
# use a model with upscaled collision geometry for the feet
Robot.urdfSuffix += "_safeFeet"
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig_legsApart[::] + [0] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
def set_joints_bounds(self):
super().set_joints_bounds()
# set conservative bounds for the leg z joint
self.fullbody.setJointBounds('leg_left_1_joint', [-0.1, 0.2])
self.fullbody.setJointBounds('leg_right_1_joint', [-0.2, 0.1])
def run(self):
self.load_fullbody()
self.set_joints_bounds()
self.set_reference(False)
self.load_limbs("fixedStep08", "ReferenceConfiguration", nb_samples=100000)
self.init_problem()
self.init_viewer()
self.compute_configs_from_guide()
# set the height to match the platefrom height
self.q_init[2] = self.q_ref[2] + 0.16
self.q_goal[2] = self.q_ref[2] + 0.16
# set right foot first
self.init_contacts = [self.fullbody.rLegId, self.fullbody.lLegId]
self.interpolate()
# remove constrained bounds on joints set before
self.fullbody.resetJointsBounds()
from scipy.spatial import ConvexHull
from talos_rbprm.talos import Robot
from .constants_and_tools import hull_to_obj
NUM_SAMPLES = 18000
IT_DISPLAY_PROGRESS = NUM_SAMPLES / 10
MIN_DIST_BETWEEN_FEET_Y = 0.18
MAX_DIST_BETWEEN_FEET_X = 0.35
MIN_HEIGHT_COM = 0.3
# margin used to constrain the com y position : if it's on the left of the left foot or on the right of the right foot
# for more than this margin, we reject this sample:
MARGIN_FEET_SIDE = 0.05
fullBody = Robot()
# fullBody.setJointBounds ("base_joint_xyz", [-2,2, -2, 2, -2, 2])
fullBody.setJointBounds("root_joint", [-20, 20, -20, 20, -20, 20])
fullBody.setConstrainedJointsBounds()
nbSamples = 1
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
v = vf.createViewer()
rootName = 'root_joint'
nbSamples = 10000
heuristic = "static"
print("gen limb db")
fullBody.addLimb(fullBody.rLegId,
from talos_rbprm.talos import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.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.85, 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])
from talos_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,
class ContactGenerator(TalosContactGenerator):
def __init__(self):
super().__init__(PathPlanner())
self.pid = 0
self.dt = 0.005
def load_fullbody(self):
from talos_rbprm.talos import Robot
Robot.urdfSuffix += "_safeFeet"
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [
0
] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [
0
] * self.path_planner.extra_dof
def set_joints_bounds(self):
super().set_joints_bounds()
self.fullbody.setConstrainedJointsBounds()
def load_limbs(self,
heuristic="fixedStep06",
analysis=None,
nb_samples=None,
octree_size=None):
# heuristic used depend on the direction of the motion
if 0.8 * np.pi > self.path_planner.alpha > 0.2 * np.pi: # nearly straight walk
print("use straight walk heuristic")
heuristic = "fixedStep08"
analysis = None
self.fullbody.usePosturalTaskContactCreation(True)
else: # more complex motion. Do smaller steps and allow non-straight feet orientation
print("use smaller steps heuristic")
analysis = "ReferenceConfiguration"
heuristic = "fixedStep06"
self.fullbody.loadAllLimbs(heuristic, analysis, nbSamples=100000)
if self.path_planner.q_goal[1] < 0:
print("start with right leg")
self.init_contacts = [self.fullbody.rLegId, self.fullbody.lLegId]
self.end_contacts = [self.fullbody.rLegId, self.fullbody.lLegId]
else:
print("start with left leg")
self.init_contacts = [self.fullbody.lLegId, self.fullbody.rLegId]
self.end_contacts = [self.fullbody.lLegId, self.fullbody.rLegId]
def compute_configs_from_guide(self):
super().compute_configs_from_guide()
vel_init = [0, 0, 0]
acc_init = [0, 0, 0]
vel_goal = [0, 0, 0]
acc_goal = [0, 0, 0]
configSize = self.fullbody.getConfigSize(
) - self.fullbody.client.robot.getDimensionExtraConfigSpace()
self.q_init[configSize:configSize + 3] = vel_init[::]
self.q_init[configSize + 3:configSize + 6] = acc_init[::]
self.q_goal[configSize:configSize + 3] = vel_goal[::]
self.q_goal[configSize + 3:configSize + 6] = acc_goal[::]
def run(self):
super().run()
self.fullbody.resetJointsBounds()
self.write_status(20)
def test_contacts_6d(self):
with ServerManager('hpp-rbprm-server'):
fullbody = Robot()
fullbody.client.robot.setDimensionExtraConfigSpace(6)
fullbody.setJointBounds("root_joint", [-10, 10, -10, 10, -10, 10])
fullbody.client.robot.setExtraConfigSpaceBounds(
[-10, 10, -10, 10, -10, 10, -10, 10, -10, 10, -10, 10])
fullbody.loadAllLimbs("static", nbSamples=10000)
q = fullbody.referenceConfig[::] + [0] * 6
fullbody.setCurrentConfig(q)
com = fullbody.getCenterOfMass()
contacts = [fullbody.rLegId, fullbody.lLegId]
state = State(fullbody, q=q, limbsIncontact=contacts)
self.assertTrue(state.isBalanced())
self.assertTrue(state.isValid()[0])
self.assertTrue(state.isLimbInContact(fullbody.rLegId))
self.assertTrue(state.isLimbInContact(fullbody.lLegId))
self.assertEqual(com, state.getCenterOfMass())
# remove rf contact :
state1, success = StateHelper.removeContact(state, fullbody.rLegId)
self.assertTrue(success)
self.assertFalse(state1.isLimbInContact(fullbody.rLegId))
self.assertTrue(state1.isLimbInContact(fullbody.lLegId))
self.assertFalse(state1.isBalanced())
self.assertEqual(com, state1.getCenterOfMass())
# create a new contact :
n = [0, 0, 1]
p = [0.15, -0.085, 0.002]
state2, success = StateHelper.addNewContact(
state1, fullbody.rLegId, p, n)
self.assertTrue(success)
self.assertTrue(state2.isLimbInContact(fullbody.rLegId))
self.assertTrue(state2.isLimbInContact(fullbody.lLegId))
self.assertTrue(state2.isBalanced())
p2, n2 = state2.getCenterOfContactForLimb(fullbody.rLegId)
self.assertEqual(n, n2)
for i in range(3):
self.assertAlmostEqual(p[i], p2[i], 2)
# try to replace a contact :
p = [0.2, 0.085, 0.002]
state3, success = StateHelper.addNewContact(
state2, fullbody.lLegId, p, n)
self.assertTrue(success)
self.assertTrue(state3.isLimbInContact(fullbody.rLegId))
self.assertTrue(state3.isLimbInContact(fullbody.lLegId))
self.assertTrue(state3.isBalanced())
# try com projection:
com_target = com[::]
com_target[2] -= 0.08
success = state.projectToCOM(com_target)
self.assertTrue(success)
fullbody.setCurrentConfig(state.q())
com_state = fullbody.getCenterOfMass()
self.assertEqual(com_state, state.getCenterOfMass())
for i in range(3):
self.assertAlmostEqual(com_state[i], com_target[i], 4)
# try lockOtherJoints:
p = [0.1, -0.085, 0.002]
state4, success = StateHelper.addNewContact(state,
fullbody.rLegId,
p,
n,
lockOtherJoints=True)
self.assertTrue(success)
self.assertTrue(state4.isLimbInContact(fullbody.rLegId))
self.assertTrue(state4.isLimbInContact(fullbody.lLegId))
self.assertTrue(state4.isBalanced())
for i in range(7, 13):
self.assertAlmostEqual(state.q()[i], state4.q()[i])
for i in range(19, len(q)):
self.assertAlmostEqual(state.q()[i], state4.q()[i])
# try with a rotation
rot = [0.259, 0, 0, 0.966]
state5, success = StateHelper.addNewContact(state,
fullbody.rLegId,
p,
n,
rotation=rot)
self.assertTrue(success)
self.assertTrue(state5.isLimbInContact(fullbody.rLegId))
self.assertTrue(state5.isLimbInContact(fullbody.lLegId))
self.assertTrue(state5.isBalanced())
fullbody.setCurrentConfig(state5.q())
rf_pose = fullbody.getJointPosition(fullbody.rfoot)
for i in range(4):
self.assertAlmostEqual(rf_pose[i + 3], rot[i], 3)