Skip to content

Commit 2ff2e5d

Browse files
EnricoMingoEnricoMingo
committed
Merge branch 'ros2' into hpipm_vangelis_dev
2 parents 068f6ef + 48a2aca commit 2ff2e5d

17 files changed

Lines changed: 211 additions & 2127 deletions

File tree

‎.github/workflows/build-and-test-ros2-noble.yml‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
name: Build and Test (Noble-ROS2)
2+
3+
on:
4+
push:
5+
branches: [ "ros2" ]
6+
pull_request:
7+
branches: [ "ros2" ]
8+
9+
jobs:
10+
11+
build:
12+
13+
runs-on: ubuntu-latest
14+
15+
steps:
16+
- uses: actions/checkout@v4
17+
- name: Build the Docker image and start the container
18+
run: docker compose up -d
19+
working-directory: ./docker/noble-ros2
20+
- name: Compile the library
21+
run: docker compose exec dev ./scripts/build.bash
22+
working-directory: ./docker/noble-ros2
23+
- name: Run tests
24+
run: docker compose exec dev ./scripts/test.bash
25+
working-directory: ./docker/noble-ros2

‎.travis.yml‎

Lines changed: 0 additions & 103 deletions
This file was deleted.

‎bindings/python/CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ if(${pybind11_FOUND})
5656

5757
install(TARGETS pyopensot_collision
5858
COMPONENT python
59-
LIBRARY DESTINATION "~/.local/lib/python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}/site-packages"
59+
LIBRARY DESTINATION "lib/python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}/site-packages"
6060
ARCHIVE DESTINATION "lib"
6161
RUNTIME DESTINATION "bin")
6262
endif()
@@ -73,7 +73,7 @@ if(${pybind11_FOUND})
7373

7474
install(TARGETS pyopensot_hcod
7575
COMPONENT python
76-
LIBRARY DESTINATION "~/.local/lib/python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}/site-packages"
76+
LIBRARY DESTINATION "lib/python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}/site-packages"
7777
ARCHIVE DESTINATION "lib"
7878
RUNTIME DESTINATION "bin")
7979
endif()

‎bindings/python/constraints/velocity.hpp‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,3 +35,58 @@ void pyVelocityOmniWheels4X(py::module& m) {
3535
.def("getIsGlobalVelocity", &OmniWheels4X::getIsGlobalVelocity);
3636

3737
}
38+
39+
using CollisionAvoidanceC = OpenSoT::constraints::velocity::CollisionAvoidance;
40+
41+
std::shared_ptr<CollisionAvoidanceC> make_collision_avoidance(
42+
const XBot::ModelInterface& model, int max_pairs,
43+
std::string collision_urdf_str, std::string collision_srdf_str)
44+
{
45+
urdf::ModelSharedPtr collision_urdf;
46+
if(!collision_urdf_str.empty()){
47+
collision_urdf = std::make_shared<urdf::Model>();
48+
if(!collision_urdf->initString(collision_urdf_str)){
49+
throw std::runtime_error("Failed to parse collision_urdf string");
50+
}
51+
}
52+
53+
srdf::ModelSharedPtr collision_srdf;
54+
if(!collision_srdf_str.empty()){
55+
collision_srdf = std::make_shared<srdf::Model>();
56+
urdf::ModelConstSharedPtr active_urdf = collision_urdf ? collision_urdf : model.getUrdf();
57+
if(!collision_srdf->initString(*active_urdf, collision_srdf_str)){
58+
throw std::runtime_error("Failed to parse collision_srdf string");
59+
}
60+
}
61+
62+
return std::make_shared<CollisionAvoidanceC>(model, max_pairs, collision_urdf, collision_srdf);
63+
}
64+
65+
66+
void pyVelocityCollisionAvoidance(py::module& m) {
67+
68+
using CollisionAvoidance = OpenSoT::constraints::velocity::CollisionAvoidance;
69+
70+
py::class_<CollisionAvoidance, std::shared_ptr<CollisionAvoidance>, OpenSoT::Constraint<Eigen::MatrixXd, Eigen::VectorXd>>(m, "CollisionAvoidance")
71+
.def(py::init(&make_collision_avoidance),
72+
py::arg(), py::arg("max_pairs") = -1, py::arg("collision_urdf") = "", py::arg("collision_srdf") = "")
73+
.def("setInfeasiblePairWeight", &CollisionAvoidance::setInfeasiblePairWeight)
74+
.def("getLinkPairThreshold", &CollisionAvoidance::getLinkPairThreshold)
75+
.def("getDetectionThreshold", &CollisionAvoidance::getDetectionThreshold)
76+
.def("setLinkPairThreshold", &CollisionAvoidance::setLinkPairThreshold)
77+
.def("setDetectionThreshold", &CollisionAvoidance::setDetectionThreshold)
78+
.def("update", &CollisionAvoidance::update)
79+
.def("setMaxPairs", &CollisionAvoidance::setMaxPairs)
80+
.def("setCollisionList", &CollisionAvoidance::setCollisionList)
81+
.def("collisionModelUpdated", &CollisionAvoidance::collisionModelUpdated)
82+
.def("addCollisionShape", &CollisionAvoidance::addCollisionShape)
83+
.def("moveCollisionShape", &CollisionAvoidance::moveCollisionShape)
84+
.def("setBoundScaling", &CollisionAvoidance::setBoundScaling)
85+
.def("setLinksVsEnvironment", &CollisionAvoidance::setLinksVsEnvironment)
86+
.def("getCollisionModel", (const XBot::Collision::CollisionModel& (CollisionAvoidance::*)() const) &CollisionAvoidance::getCollisionModel)
87+
.def("getOrderedWitnessPointVector", &CollisionAvoidance::getOrderedWitnessPointVector)
88+
.def("getOrderedLinkPairVector", &CollisionAvoidance::getOrderedLinkPairVector)
89+
.def("getOrderedDistanceVector", &CollisionAvoidance::getOrderedDistanceVector)
90+
.def("getCollisionJacobian", &CollisionAvoidance::getCollisionJacobian);
91+
}
92+

‎bindings/python/examples/tiago_dual_collision_avoidance.py‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -244,11 +244,12 @@ def publishCollisionDistances(self, collision_distance_points, time):
244244
dt = 1./100.
245245

246246
## CREATE OPTIMIZATION PROBLEM
247+
manipulation_base_frame = "world"
247248
#1. TASKS
248-
gripper_left = Cartesian("Cartesian", model, "gripper_left_grasping_frame", "base_link")
249+
gripper_left = Cartesian("Cartesian", model, "gripper_left_grasping_frame", manipulation_base_frame)
249250
gripper_left.setLambda(0.1)
250251

251-
gripper_right = Cartesian("Cartesian", model, "gripper_right_grasping_frame", "base_link")
252+
gripper_right = Cartesian("Cartesian", model, "gripper_right_grasping_frame", manipulation_base_frame)
252253
gripper_right.setLambda(0.1)
253254

254255
base = Cartesian("Cartesian", model, "base_link", "world")
@@ -360,7 +361,7 @@ def publishCollisionDistances(self, collision_distance_points, time):
360361
pose_ref, vel_ref = gripper_right.getReference()
361362
print(f"pose_ref: {pose_ref}")
362363
print(f"vel_ref: {vel_ref}")
363-
node.make_6dof_marker(name="gripper_right_marker", pose=pose_ref, frame_id="base_link")
364+
node.make_6dof_marker(name="gripper_right_marker", pose=pose_ref, frame_id=manipulation_base_frame)
364365

365366
object_in_scene = False
366367
try:
@@ -434,4 +435,4 @@ def publishCollisionDistances(self, collision_distance_points, time):
434435
node.destroy_node()
435436

436437
if rclpy.ok():
437-
rclpy.shutdown()
438+
rclpy.shutdown()

‎docker/noble-ros2/Dockerfile‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
FROM hhcmhub/xbot2-noble-dev

‎docker/noble-ros2/compose.yaml‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
services:
2+
dev:
3+
build: .
4+
stdin_open: true
5+
tty: true
6+
entrypoint: /bin/bash
7+
restart: always
8+
volumes:
9+
- /home/$USER/.ssh:/home/user/.ssh
10+
- ./scripts:/home/user/scripts
11+
- ../..:/home/user/test_ws/src/OpenSoT:ro
12+
environment:
13+
- TERM=xterm-256color

‎docker/noble-ros2/scripts/build.bash‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
#!/bin/bash
2+
set -e
3+
4+
# make forest https
5+
export HHCM_FOREST_CLONE_DEFAULT_PROTO=https
6+
7+
# activate python venv
8+
source env/bin/activate
9+
10+
# create ws and source it
11+
sudo chown user:user test_ws test_ws/src
12+
mkdir -p test_ws && cd test_ws
13+
forest init
14+
15+
# setup env
16+
source /opt/ros/jazzy/setup.bash
17+
source setup.bash
18+
19+
# here we want to check that opensot builds and works against the binaries of the
20+
# xbot dependencies
21+
source /opt/xbot/setup.sh
22+
sudo apt remove -y open_sot
23+
24+
# add recipes
25+
forest add-recipes git@github.com:advrhumanoids/multidof_recipes.git -t ros2
26+
27+
# build
28+
export PYTHONUNBUFFERED=1
29+
forest grow OpenSoT --verbose --clone-depth 1 -j ${FOREST_JOBS:-1}
30+
31+
# build tests
32+
cd build/OpenSoT
33+
cmake -DOPENSOT_COMPILE_TESTS=1 .
34+
make -j ${FOREST_JOBS:-1}

‎docker/noble-ros2/scripts/test.bash‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
#!/bin/bash
2+
set -e
3+
4+
# setup env
5+
source /opt/ros/jazzy/setup.bash
6+
source ~/test_ws/setup.bash
7+
source /opt/xbot/setup.sh
8+
9+
# run tests
10+
cd ~/test_ws/build/OpenSoT
11+
ctest --output-on-failure

‎include/OpenSoT/constraints/velocity/CollisionAvoidance.h‎

Lines changed: 13 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -60,8 +60,17 @@ class CollisionAvoidance: public Constraint<Eigen::MatrixXd, Eigen::VectorXd>
6060
CollisionAvoidance(const XBot::ModelInterface& robot,
6161
int max_pairs = -1,
6262
urdf::ModelConstSharedPtr collision_urdf = nullptr,
63-
srdf::ModelConstSharedPtr collision_srdf = nullptr,
64-
bool skip_infeasible_pairs = true);
63+
srdf::ModelConstSharedPtr collision_srdf = nullptr);
64+
65+
66+
67+
/**
68+
* @brief scale velocity generated by infeasible pairs to avoid infeasibilities
69+
* the default value is 0.0, which means that infeasible pairs are not considered
70+
* (collision states will not be recovered)
71+
* @param w
72+
*/
73+
void setInfeasiblePairWeight(double w);
6574

6675
/**
6776
* @brief getLinkPairThreshold distance offset between two link pairs
@@ -206,9 +215,9 @@ class CollisionAvoidance: public Constraint<Eigen::MatrixXd, Eigen::VectorXd>
206215
int _max_pairs;
207216

208217
/**
209-
* @brief _skip_infeasible_pairs
218+
* @brief _infeasible_pair_weight
210219
*/
211-
bool _skip_infeasible_pairs;
220+
double _infeasible_pair_weight;
212221

213222
/**
214223
* @brief _robot

0 commit comments

Comments
 (0)