Skip to content

GitLab

Commit a69dd94e authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge tag 'v4.5.0' 

Release of version 4.5.0.
parents c993904d 8f598dbe
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ a69dd94e
......@@ -51,6 +51,7 @@ ADD_REQUIRED_DEPENDENCY("hpp-rbprm >= 4.3")
ADD_REQUIRED_DEPENDENCY("omniORB4 >= 4.1.4")
ADD_REQUIRED_DEPENDENCY("hpp-affordance-corba")
ADD_REQUIRED_DEPENDENCY("hpp-util >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-pinocchio >= 4.3")
set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/find-external/CDD")
find_package(CDD REQUIRED)
......
cmake @ 61e5574a
Compare 5c8c19f4...61e5574a
Subproject commit 5c8c19f491f2c6f8488f5f37ff81d711d69dbb3f
Subproject commit 61e5574a0615706aab06986f6aecf665ddc31141
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @ a69dd94e
......@@ -18,7 +18,7 @@
#ifndef HPP_RBPRM_CORBA_BUILDER_IDL
# define HPP_RBPRM_CORBA_BUILDER_IDL
# include <hpp/corbaserver/common.idl>
# include <hpp/common.idl>
module hpp
{
......@@ -106,6 +106,10 @@ module hpp
void setReferenceConfig(in floatSeq referenceConfig)
raises (Error);
/// set the weights used when computing a postural task
void setPostureWeights(in floatSeq postureWeights)
raises (Error);
/// set a reference position of the end effector for the given ROM
void setReferenceEndEffector(in string romName, in floatSeq ref)
raises (Error);
......
script/scenarios/demos/run.sh
View file @ a69dd94e
#!/bin/bash
gepetto-viewer-server &
gepetto-gui &
hpp-rbprm-server &
ipython -i --no-confirm-exit ./$1
pkill -f 'gepetto-viewer-server'
pkill -f 'gepetto-gui'
pkill -f 'hpp-rbprm-server'
script/scenarios/demos/rund.sh
View file @ a69dd94e
#!/bin/bash
gepetto-viewer-server &
gepetto-gui &
ipython -i --no-confirm-exit ./$1
pkill -f 'gepetto-viewer-server'
pkill -f 'gepetto-gui'
script/scenarios/sandbox/dynamic/tools.py deleted 100644 → 0
View file @ c993904d
def addSphere(viewer,color, pos,rotation = None,name=None,radius=0.01):
gui = viewer.client.gui
if name==None:
i=0
name='sphere_'+str(i)
while name in gui.getNodeList():
i=i+1
name='sphere_'+str(i)
gui.addSphere(name,radius,color)
gui.setVisibility(name,"ALWAYS_ON_TOP")
gui.addToGroup(name,viewer.sceneName)
if len(pos)==7:
rotation=pos[3:7]
pos=pos[0:3]
if rotation==None:
rotation=[1,0,0,0]
else:
viewer.addLandmark(name,0.1)
gui.applyConfiguration(name,pos+rotation)
gui.refresh()
def moveObject(viewer,pos,rotation=[1,0,0,0]):
viewer.client.gui.applyConfiguration(name,pos+rotation)
viewer.client.gui.refresh()
def addVector(viewer,rbprmBuilder,color,v,name=None):
gui = viewer.client.gui
if name==None:
i=0
name='vector_'+str(i)
while name in gui.getNodeList():
i=i+1
name='sphere_'+str(i)
quat = rbprmBuilder.quaternionFromVector(v[3:6])
v[3:7] = quat[::]
gui.addArrow(name,0.02,1,color)
gui.addToGroup(name,viewer.sceneName)
gui.setVisibility(name,"ON")
gui.applyConfiguration(name,v)
gui.refresh()
script/tools/constraint_to_dae.py
View file @ a69dd94e
import subprocess
import os
DIR = "/local/fernbac/bench_iros18/constraints_obj/"
DIR = "/local/fernbach/qhull/constraints_obj/"
STAB_NAME = "stability"
CONS_NAME = "constraints"
KIN_NAME = "kinematics"
......@@ -10,8 +10,9 @@ BEZIER_NAME = "bezier_wp"
def generate_off_file(name):
os.remove(DIR+name+"_.off") if os.path.isfile(DIR+name+"_.off") else None
os.remove(DIR+name+".off") if os.path.isfile(DIR+name+".off") else None
cmd = "cat "+DIR+name+".txt | qhalf Fp | qconvex o >> "+DIR+name+"_.off"
#cmd = "cat "+DIR+name+".txt | qhalf Fp | qconvex o >> "+DIR+name+"_.off"
cmd = "cat "+DIR+name+".txt | qhalf FP | qconvex Ft >> "+DIR+name+"_.off"
try :
subprocess.check_output(cmd,shell=True)
print "qHull OK for file : "+name+".txt"
......
script/tools/createActionDRP.py 0 → 100644
View file @ a69dd94e
# Create actions YAML file
import yaml
#import anymal_darpa_Asil as drp
def autoBaseFootstep(configs,fullbody,filename):
initial = """\
steps:
- step:
- base_auto:
height:
average_linear_velocity: 0.5
- step:
- base_auto:
average_linear_velocity: 0.5
- footstep:
name: RF_LEG
target:
frame: odom
position:
- step:
- base_auto:
average_linear_velocity: 0.5
- step:
- base_auto:
average_linear_velocity: 0.5
- footstep:
name: LH_LEG
target:
frame: odom
position:
- step:
- base_auto:
average_linear_velocity: 0.5
- step:
- base_auto:
average_linear_velocity: 0.5
- footstep:
name: LF_LEG
target:
frame: odom
position:
- step:
- base_auto:
average_linear_velocity: 0.5
- step:
- base_auto:
average_linear_velocity: 0.5
- footstep:
name: RH_LEG
target:
frame: odom
position:
- step:
- base_auto:
average_linear_velocity: 0.5
"""
data = yaml.load(initial)
LF_footPos = fullbody.getEffectorPosition("LFleg", configs[0])
LH_footPos = fullbody.getEffectorPosition("LHleg", configs[0])
RF_footPos = fullbody.getEffectorPosition("RFleg", configs[0])
RH_footPos = fullbody.getEffectorPosition("RHleg", configs[0])
data['steps'][0]['step'][0]['base_auto']['height'] = configs[0][2]
data['steps'][1]['step'][1]['footstep']['target']['position'] = RF_footPos[0]
data['steps'][3]['step'][1]['footstep']['target']['position'] = LH_footPos[0]
data['steps'][5]['step'][1]['footstep']['target']['position'] = LF_footPos[0]
data['steps'][7]['step'][1]['footstep']['target']['position'] = RH_footPos[0]
for i in range(1, len(configs)):
step = """\
- step:
- base_auto:
average_linear_velocity: 0.5
- footstep:
name:
target:
frame: odom
position:
- step:
- base_auto:
average_linear_velocity: 0.5
"""
step_data = yaml.load(step)
if i == (len(configs)-1):
footPos = fullbody.getEffectorPosition("RFleg", configs[i])
step_data[0]['step'][1]['footstep']['name'] = 'RF_LEG'
step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
footPos = fullbody.getEffectorPosition("LHleg", configs[i])
step_data[0]['step'][1]['footstep']['name'] = 'LH_LEG'
step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
footPos = fullbody.getEffectorPosition("LFleg", configs[i])
step_data[0]['step'][1]['footstep']['name'] = 'LF_LEG'
step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
footPos = fullbody.getEffectorPosition("RHleg", configs[i])
step_data[0]['step'][1]['footstep']['name'] = 'RH_LEG'
step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
movedLeg = fullbody.getContactsVariations(i-1, i)
if movedLeg[0] == 'LFleg':
footPos = fullbody.getEffectorPosition("LFleg", configs[i])
step_data[0]['step'][1]['footstep']['name'] = 'LF_LEG'
step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
elif movedLeg[0] == 'LHleg':
footPos = fullbody.getEffectorPosition("LHleg", configs[i])
step_data[0]['step'][1]['footstep']['name'] = 'LH_LEG'
step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
elif movedLeg[0] == 'RFleg':
footPos = fullbody.getEffectorPosition("RFleg", configs[i])
step_data[0]['step'][1]['footstep']['name'] = 'RF_LEG'
step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
elif movedLeg[0] == 'RHleg':
footPos = fullbody.getEffectorPosition("RHleg", configs[i])
step_data[0]['step'][1]['footstep']['name'] = 'RH_LEG'
step_data[0]['step'][1]['footstep']['target']['position'] = footPos[0]
data['steps'].extend(step_data)
stream = file(filename+'.yaml', 'w')
yaml.dump(data, stream, default_flow_style=False)
print('saved .yaml file as '+filename+'.yaml')
script/tools/display_tools.py
View file @ a69dd94e
......@@ -16,3 +16,25 @@ def displayContactSequence(r,configs,pause=1.):
for i in range(0,len(configs)):
r(configs[i])
time.sleep(pause)
def moveObject(viewer,pos,rotation=[1,0,0,0]):
viewer.client.gui.applyConfiguration(name,pos+rotation)
viewer.client.gui.refresh()
def addVector(viewer,rbprmBuilder,color,v,name=None):
gui = viewer.client.gui
if name==None:
i=0
name='vector_'+str(i)
while name in gui.getNodeList():
i=i+1
name='sphere_'+str(i)
quat = rbprmBuilder.quaternionFromVector(v[3:6])
v[3:7] = quat[::]
gui.addArrow(name,0.02,1,color)
gui.addToGroup(name,viewer.sceneName)
gui.setVisibility(name,"ON")
gui.applyConfiguration(name,v)
gui.refresh()
src/CMakeLists.txt
View file @ a69dd94e
......@@ -20,10 +20,6 @@ SET(IDL_SOURCES
rbprmbuilder
)
SET(HPP_CORBASERVER_IDL_SOURCES
common
)
OMNIIDL_INCLUDE_DIRECTORIES(
${HPP_CORBASERVER_DATAROOTDIR}/idl ${CMAKE_SOURCE_DIR}/idl
)
......@@ -33,46 +29,33 @@ FINDPYTHON(2.7 EXACT REQUIRED)
INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR}/src)
FILE(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/rbprm)
FOREACH(IDL ${IDL_SOURCES})
GENERATE_IDL_CPP (${IDL}
${CMAKE_SOURCE_DIR}/idl/hpp/corbaserver/rbprm
)
GENERATE_IDL_PYTHON (${IDL}
${CMAKE_SOURCE_DIR}/idl/hpp/corbaserver/rbprm
)
ENDFOREACH()
FOREACH(IDL ${IDL_SOURCES})
GENERATE_IDL_CPP (hpp/corbaserver/rbprm/${IDL}
${CMAKE_SOURCE_DIR}/idl/hpp/corbaserver/rbprm)
GENERATE_IDL_PYTHON (hpp/corbaserver/rbprm/${IDL}
${CMAKE_SOURCE_DIR}/idl/hpp/corbaserver/rbprm)
${CMAKE_SOURCE_DIR}/idl/hpp/corbaserver/rbprm
HEADER_SUFFIX -idl.hh)
GENERATE_IDL_PYTHON (${IDL}
${CMAKE_SOURCE_DIR}/idl/hpp/corbaserver/rbprm
ENABLE_DOCSTRING
STUBS hpp_stubs.rbprm
ARGUMENTS
-Wbmodules=hpp_idl
-Wbextern=common:hpp_stubs
)
INSTALL(
FILES ${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/rbprm/${IDL}.hh
FILES ${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/rbprm/${IDL}-idl.hh
DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/hpp/corbaserver/rbprm)
INSTALL(
FILES ${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/rbprm/${IDL}_idl.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm)
ENDFOREACH()
FOREACH(IDL ${HPP_CORBASERVER_IDL_SOURCES})
GENERATE_IDL_CPP (${IDL}
${HPP_CORBASERVER_DATAROOTDIR}/idl/hpp/corbaserver
)
GENERATE_IDL_PYTHON (${IDL}
${HPP_CORBASERVER_DATAROOTDIR}/idl/hpp/corbaserver
)
INSTALL(FILES
${CMAKE_CURRENT_BINARY_DIR}/${IDL}_idl.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm)
ENDFOREACH()
INSTALL(
DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/hpp_idl/hpp/corbaserver/rbprm
DESTINATION ${PYTHON_SITELIB}/hpp_idl/hpp/corbaserver)
INSTALL(
DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/hpp_stubs/rbprm
DESTINATION ${PYTHON_SITELIB}/hpp_stubs)
ADD_LIBRARY(${LIBRARY_NAME} SHARED
common.hh
commonSK.cc
${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/rbprm/rbprmbuilder.hh
${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/rbprm/rbprmbuilderSK.cc
${ALL_IDL_CPP_STUBS}
rbprmbuilder.impl.cc
rbprmbuilder.impl.hh
server.cc
......@@ -123,5 +106,6 @@ INSTALL(
# Stand alone corba server
ADD_EXECUTABLE (hpp-rbprm-server hpp-rbprm-corba.cc)
TARGET_LINK_LIBRARIES (hpp-rbprm-server ${LIBRARY_NAME} hpp-rbprm)
PKG_CONFIG_USE_DEPENDENCY(hpp-rbprm-server hpp-pinocchio)
INSTALL (TARGETS hpp-rbprm-server DESTINATION ${CMAKE_INSTALL_BINDIR})
src/hpp/corbaserver/rbprm/__init__.py
View file @ a69dd94e
import omniORB
omniORB.updateModule("hpp.corbaserver.rbprm")
import rbprmbuilder_idl
from client import Client
src/hpp/corbaserver/rbprm/client.py
View file @ a69dd94e
......@@ -17,51 +17,29 @@
# hpp-manipulation-corba. If not, see
# <http://www.gnu.org/licenses/>.
from omniORB import CORBA
import CosNaming
from hpp.corbaserver.client import Client as _Parent
from hpp_idl.hpp.corbaserver.rbprm import RbprmBuilder
from hpp.corbaserver.rbprm import RbprmBuilder
class CorbaError(Exception):
"""
Raised when a CORBA error occurs.
"""
def __init__(self, value):
self.value = value
def __str__(self):
return repr(self.value)
class Client:
class Client (_Parent):
"""
Connect and create clients for hpp-rbprm library.
"""
def __init__(self):
defaultClients = {
'rbprmbuilder' : RbprmBuilder,
}
def __init__(self, url = None, context = "corbaserver"):
"""
Initialize CORBA and create default clients.
:param url: URL in the IOR, corbaloc, corbalocs, and corbanames formats.
For a remote corba server, use
url = "corbaloc:iiop:<host>:<port>/NameService"
"""
import sys
self.orb = CORBA.ORB_init (sys.argv, CORBA.ORB_ID)
obj = self.orb.resolve_initial_references("NameService")
self.rootContext = obj._narrow(CosNaming.NamingContext)
if self.rootContext is None:
raise CorbaError ('failed to narrow the root context')
# client of Rbprm interface
name = [CosNaming.NameComponent ("hpp", "corbaserver"),
CosNaming.NameComponent ("rbprm", "rbprmbuilder")]
try:
obj = self.rootContext.resolve (name)
except CosNaming.NamingContext.NotFound, ex:
raise CorbaError ('failed to find rbprm service.')
try:
client = obj._narrow (RbprmBuilder)
except KeyError:
raise CorbaError ('invalid service name rbprm')
if client is None:
# This happens when stubs from client and server are not synchronized.
raise CorbaError (
'failed to narrow client for service rbprm')
self.rbprm = client
self._initOrb (url)
self._makeClients ("rbprm", self.defaultClients, context)
# self.rbprmbuilder is created by self._makeClients
# The old code stored the object as self.rbprm
# Make it backward compatible.
self.rbprm = self.rbprmbuilder
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @ a69dd94e
......@@ -18,7 +18,6 @@
from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.corbaserver.robot import Robot
import hpp.gepetto.blender.exportmotion as em
from numpy import array, matrix
from hpp_spline import bezier
......@@ -817,6 +816,7 @@ class FullBody (Robot):
# \param configurations list of configurations to save
# \param filename outputfile where to export the motion
def exportMotion(self, viewer, configurations, filename):
import hpp.gepetto.blender.exportmotion as em
em.exportStates(viewer, self.client.robot, configurations, filename)
## Export motion to a format readable by the blender
......@@ -843,6 +843,11 @@ class FullBody (Robot):
def setReferenceConfig(self,referenceConfig):
return self.clientRbprm.rbprm.setReferenceConfig(referenceConfig)
## set the weights used when computing a postural task
# \param postureWeights dofArray, must be of same size as device->numberDof
def setPostureWeights(self,postureWeights):
return self.clientRbprm.rbprm.setPostureWeights(postureWeights)
## return the time at the given state index (in the path computed during the first phase)
# \param stateId : index of the state
def getTimeAtState(self,stateId):
......
src/rbprmbuilder.impl.cc
View file @ a69dd94e
......@@ -17,7 +17,7 @@
//#include <hpp/fcl/math/transform.h>
#include <hpp/util/debug.hh>
#include <hpp/corbaserver/rbprm/rbprmbuilder.hh>
#include <hpp/corbaserver/rbprm/rbprmbuilder-idl.hh>
#include "rbprmbuilder.impl.hh"
#include "hpp/rbprm/rbprm-device.hh"
#include "hpp/rbprm/rbprm-validation.hh"
......@@ -633,6 +633,13 @@ namespace hpp {
fullBody()->referenceConfig(config);
}
void RbprmBuilder::setPostureWeights(const hpp::floatSeq& postureWeights) throw (hpp::Error){
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
Configuration_t config(dofArrayToConfig (fullBody()->device_->numberDof(), postureWeights));
fullBody()->postureWeights(config);
}
void RbprmBuilder::setReferenceEndEffector(const char* romName, const hpp::floatSeq &ref) throw(hpp::Error){
std::string name (romName);
hpp::pinocchio::RbPrmDevicePtr_t device = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
......@@ -2552,20 +2559,21 @@ namespace hpp {
State s2 = lastStatesComputed_[size_t(state2)];
hppDout(notice,"state1 = r(["<<pinocchio::displayConfig(s1.configuration_)<<")]");
hppDout(notice,"state2 = r(["<<pinocchio::displayConfig(s2.configuration_)<<")]");
core::PathVectorPtr_t resPath = core::PathVector::create(fullBody()->device_->configSize(), fullBody()->device_->numberDof());
//core::PathVectorPtr_t resPath = core::PathVector::create(fullBody()->device_->configSize(), fullBody()->device_->numberDof());
std::vector<CORBA::Short> pathsIds;
core::PathPtr_t p1 = (*functor)(fullBody(),problemSolver(), paths[comTraj],
s1,s2, numOptimizations,true);
s1,s2, numOptimizations,false);
hppDout(notice,"effectorRRT done.");
// reduce path to remove extradof
core::segment_t interval(0, p1->initial().rows()-1);
/*core::segment_t interval(0, p1->initial().rows()-1);
core::segments_t intervals;
intervals.push_back(interval);
core::segments_t velIntervals (1, core::segment_t (0, fullBody()->device_->numberDof()));
PathPtr_t reducedPath = core::SubchainPath::create(p1,intervals, velIntervals);
resPath->appendPath(reducedPath);
pathsIds.push_back((CORBA::Short)problemSolver()->addPath(resPath));
*/
pathsIds.push_back((CORBA::Short)AddPath(p1,problemSolver()));
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length((_CORBA_ULong)pathsIds.size());
......
src/rbprmbuilder.impl.hh
View file @ a69dd94e
......@@ -20,7 +20,7 @@
# include <hpp/core/problem-solver.hh>
# include <hpp/core/path.hh>
# include "rbprmbuilder.hh"
# include "hpp/corbaserver/rbprm/rbprmbuilder-idl.hh"
# include <hpp/rbprm/rbprm-device.hh>
# include <hpp/rbprm/rbprm-fullbody.hh>
# include <hpp/rbprm/rbprm-shooter.hh>
......@@ -196,6 +196,7 @@ namespace hpp {
void setStaticStability(const bool staticStability) throw (hpp::Error);
void setReferenceConfig(const hpp::floatSeq &referenceConfig) throw (hpp::Error);
void setPostureWeights(const hpp::floatSeq &postureWeights) throw (hpp::Error);
void setReferenceEndEffector(const char* romName, const hpp::floatSeq &ref) throw(hpp::Error);
virtual void setFilter(const hpp::Names_t& roms) throw (hpp::Error);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment