diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..81412e3
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,213 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(chonk_pushing)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ xacro
+ pushing_msgs
+ kdl_parser
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES chonk_pushing
+# CATKIN_DEPENDS other_catkin_pkg
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+file(GLOB xacro_files urdf/*.urdf.xacro)
+xacro_add_files(${xacro_files} TARGET urdf INSTALL)
+
+install(DIRECTORY urdf
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/chonk_pushing.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/chonk_pushing_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_chonk_pushing.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/launch/action_servers_4tasks.launch b/launch/action_servers_4tasks.launch
new file mode 100644
index 0000000..1a6fe24
--- /dev/null
+++ b/launch/action_servers_4tasks.launch
@@ -0,0 +1,79 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC.launch b/launch/action_servers_MPC.launch
new file mode 100644
index 0000000..1794c0c
--- /dev/null
+++ b/launch/action_servers_MPC.launch
@@ -0,0 +1,79 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC.launch b/launch/action_servers_MPC_BC.launch
new file mode 100644
index 0000000..69a4df7
--- /dev/null
+++ b/launch/action_servers_MPC_BC.launch
@@ -0,0 +1,81 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC2.launch b/launch/action_servers_MPC_BC2.launch
new file mode 100644
index 0000000..28d9e67
--- /dev/null
+++ b/launch/action_servers_MPC_BC2.launch
@@ -0,0 +1,79 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational.launch b/launch/action_servers_MPC_BC_operational.launch
new file mode 100644
index 0000000..cb3928e
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational.launch
@@ -0,0 +1,85 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_AD_obstable_pick.launch b/launch/action_servers_MPC_BC_operational_AD_obstable_pick.launch
new file mode 100644
index 0000000..84e95a6
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_AD_obstable_pick.launch
@@ -0,0 +1,83 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_AD_obstable_pick_longhorizon.launch b/launch/action_servers_MPC_BC_operational_AD_obstable_pick_longhorizon.launch
new file mode 100644
index 0000000..632d202
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_AD_obstable_pick_longhorizon.launch
@@ -0,0 +1,83 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_AD_pick.launch b/launch/action_servers_MPC_BC_operational_AD_pick.launch
new file mode 100644
index 0000000..d98e93e
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_AD_pick.launch
@@ -0,0 +1,83 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_TC.launch b/launch/action_servers_MPC_BC_operational_TC.launch
new file mode 100644
index 0000000..59615e3
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_TC.launch
@@ -0,0 +1,97 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_obstable_sensorAD_notGood.launch b/launch/action_servers_MPC_BC_operational_obstable_sensorAD_notGood.launch
new file mode 100644
index 0000000..136fa1f
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_obstable_sensorAD_notGood.launch
@@ -0,0 +1,83 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_pick_experiment.launch b/launch/action_servers_MPC_BC_operational_pick_experiment.launch
new file mode 100644
index 0000000..49a4cd6
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_pick_experiment.launch
@@ -0,0 +1,69 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_pick_experiment_bigbox.launch b/launch/action_servers_MPC_BC_operational_pick_experiment_bigbox.launch
new file mode 100644
index 0000000..0d0701c
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_pick_experiment_bigbox.launch
@@ -0,0 +1,66 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_pick_localsensorAD.launch b/launch/action_servers_MPC_BC_operational_pick_localsensorAD.launch
new file mode 100644
index 0000000..c4ef7f1
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_pick_localsensorAD.launch
@@ -0,0 +1,96 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.launch b/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.launch
new file mode 100644
index 0000000..89d9b60
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.launch
@@ -0,0 +1,96 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.launch b/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.launch
new file mode 100644
index 0000000..6d46f61
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.launch
@@ -0,0 +1,97 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment.launch b/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment.launch
new file mode 100644
index 0000000..6d46f61
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment.launch
@@ -0,0 +1,97 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.launch b/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.launch
new file mode 100644
index 0000000..f05c664
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.launch
@@ -0,0 +1,96 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_pick_sensorAD.launch b/launch/action_servers_MPC_BC_operational_pick_sensorAD.launch
new file mode 100644
index 0000000..86c0e92
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_pick_sensorAD.launch
@@ -0,0 +1,96 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.launch b/launch/action_servers_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.launch
new file mode 100644
index 0000000..4aa34b0
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.launch
@@ -0,0 +1,96 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.launch b/launch/action_servers_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.launch
new file mode 100644
index 0000000..33632e7
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.launch
@@ -0,0 +1,96 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.launch b/launch/action_servers_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.launch
new file mode 100644
index 0000000..ceb362f
--- /dev/null
+++ b/launch/action_servers_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.launch
@@ -0,0 +1,96 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_fixedbase.launch b/launch/action_servers_fixedbase.launch
new file mode 100644
index 0000000..03495a3
--- /dev/null
+++ b/launch/action_servers_fixedbase.launch
@@ -0,0 +1,78 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_planner.launch b/launch/action_servers_planner.launch
new file mode 100644
index 0000000..962fb03
--- /dev/null
+++ b/launch/action_servers_planner.launch
@@ -0,0 +1,79 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/action_servers_wholebody.launch b/launch/action_servers_wholebody.launch
new file mode 100644
index 0000000..ec12d70
--- /dev/null
+++ b/launch/action_servers_wholebody.launch
@@ -0,0 +1,69 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_4tasks.launch b/launch/gazebo_4tasks.launch
new file mode 100644
index 0000000..3053620
--- /dev/null
+++ b/launch/gazebo_4tasks.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_fixedbase.launch b/launch/gazebo_fixedbase.launch
new file mode 100644
index 0000000..e60160f
--- /dev/null
+++ b/launch/gazebo_fixedbase.launch
@@ -0,0 +1,80 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner.launch b/launch/gazebo_planner.launch
new file mode 100644
index 0000000..1ca26d7
--- /dev/null
+++ b/launch/gazebo_planner.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC.launch b/launch/gazebo_planner_MPC.launch
new file mode 100644
index 0000000..1ca26d7
--- /dev/null
+++ b/launch/gazebo_planner_MPC.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC.launch b/launch/gazebo_planner_MPC_BC.launch
new file mode 100644
index 0000000..1ca26d7
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC2.launch b/launch/gazebo_planner_MPC_BC2.launch
new file mode 100644
index 0000000..1ca26d7
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC2.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational.launch b/launch/gazebo_planner_MPC_BC_operational.launch
new file mode 100644
index 0000000..119250c
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_AD_obstable_pick.launch b/launch/gazebo_planner_MPC_BC_operational_AD_obstable_pick.launch
new file mode 100644
index 0000000..469bd36
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_AD_obstable_pick.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_AD_pick.launch b/launch/gazebo_planner_MPC_BC_operational_AD_pick.launch
new file mode 100644
index 0000000..780a2c4
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_AD_pick.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_TC.launch b/launch/gazebo_planner_MPC_BC_operational_TC.launch
new file mode 100644
index 0000000..6a67716
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_TC.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_experiment.launch b/launch/gazebo_planner_MPC_BC_operational_experiment.launch
new file mode 100644
index 0000000..b8dc714
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_experiment.launch
@@ -0,0 +1,84 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_obstable_sensorAD_notGood.launch b/launch/gazebo_planner_MPC_BC_operational_obstable_sensorAD_notGood.launch
new file mode 100644
index 0000000..469bd36
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_obstable_sensorAD_notGood.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD.launch b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD.launch
new file mode 100644
index 0000000..c306531
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.launch b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.launch
new file mode 100644
index 0000000..2314798
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon_turn.launch b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon_turn.launch
new file mode 100644
index 0000000..009ff81
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon_turn.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.launch b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.launch
new file mode 100644
index 0000000..e6c320a
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn.launch b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn.launch
new file mode 100644
index 0000000..c428834
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.launch b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.launch
new file mode 100644
index 0000000..e6c320a
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn.launch b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn.launch
new file mode 100644
index 0000000..c428834
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_turn.launch b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_turn.launch
new file mode 100644
index 0000000..22e8318
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_turn.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD.launch b/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD.launch
new file mode 100644
index 0000000..c306531
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.launch b/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.launch
new file mode 100644
index 0000000..2314798
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.launch b/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.launch
new file mode 100644
index 0000000..e6c320a
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.launch b/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.launch
new file mode 100644
index 0000000..e6c320a
--- /dev/null
+++ b/launch/gazebo_planner_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_test.launch b/launch/gazebo_test.launch
new file mode 100644
index 0000000..ab4c278
--- /dev/null
+++ b/launch/gazebo_test.launch
@@ -0,0 +1,81 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_wholebody.launch b/launch/gazebo_wholebody.launch
new file mode 100644
index 0000000..1ca26d7
--- /dev/null
+++ b/launch/gazebo_wholebody.launch
@@ -0,0 +1,82 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/robot.launch b/launch/robot.launch
new file mode 100644
index 0000000..92534fd
--- /dev/null
+++ b/launch/robot.launch
@@ -0,0 +1,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/teleop2d.launch b/launch/teleop2d.launch
new file mode 100644
index 0000000..fc2817f
--- /dev/null
+++ b/launch/teleop2d.launch
@@ -0,0 +1,17 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/package.xml b/package.xml
new file mode 100644
index 0000000..27a4ca8
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,64 @@
+
+
+ chonk_pushing
+ 0.0.0
+ The chonk_pushing package
+
+
+
+
+ Joao Moura
+
+
+
+
+
+ BSD
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+
+ chonk_description
+ nextage_extensions
+ pushing_msgs
+ rospy
+ netft_rdt_driver
+
+
+
+
+
+
+
diff --git a/script/EVA_dynamics_calculation_notGood.py b/script/EVA_dynamics_calculation_notGood.py
new file mode 100755
index 0000000..314f183
--- /dev/null
+++ b/script/EVA_dynamics_calculation_notGood.py
@@ -0,0 +1,235 @@
+#! /usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+To run this example, first install iDynTree for python,
+then navigate to this directory and run:
+python KinDynComputationsTutorial.py
+
+Otherwise, modify the URDF_FILE parameter to point to an existing
+URDF in your system.
+"""
+"""
+Notice: By using idyntree, including base linear motion, angular motion, in this sequence.
+For C and G, it includes linear force, and angular force, in this sequence.
+"""
+import rospy
+import idyntree.bindings as iDynTree
+import numpy as np
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import JointState
+import tf
+import optas
+import scipy
+
+
+class InertiaProcessing:
+ def __init__(self):
+ # Initialize the ROS node
+ rospy.init_node('inertia_processor')
+ self.rate = rospy.Rate(100)
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare inertia publishers
+ self._inertia_pub = rospy.Publisher("/chonk/arm_ee_inertias", Float64MultiArray, queue_size=10)
+ # initialize the two arm ee grasp inertia message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = 6 + 6 # meaning: 6 for right arm ee grasp, 6 for left arm ee grasp
+ # declare inertia publishers
+ self._inertia_1DOF_pub = rospy.Publisher("/chonk/arm_ee_1DOF_inertias_1DOF", Float64MultiArray, queue_size=10)
+ # initialize the two arm ee grasp inertia message
+ self._msg_1DOF = Float64MultiArray()
+ self._msg_1DOF.layout = MultiArrayLayout()
+ self._msg_1DOF.layout.data_offset = 0
+ self._msg_1DOF.layout.dim.append(MultiArrayDimension())
+ self._msg_1DOF.layout.dim[0].label = "columns"
+ self._msg_1DOF.layout.dim[0].size = 1 + 1 # meaning: 6 for right arm ee grasp, 6 for left arm ee grasp
+ # define the urdf file used by idyntree, this urdf is generated from gazebo
+ self.URDF_FILE = '/home/dwq/chonk/src/chonk_pushing/urdf/chonk_gazebo_fixed_wheels_combinebody.urdf'
+ # load urdf model
+ self.dynComp = iDynTree.KinDynComputations()
+ self.mdlLoader = iDynTree.ModelLoader()
+ self.mdlLoader.loadModelFromFile(self.URDF_FILE)
+ self.dynComp.loadRobotModel(self.mdlLoader.model())
+ self.model = self.dynComp.model()
+ # set floating-base link
+ self.dynComp.setFloatingBase("donkey_link")
+ # set the frame reference, all expressed in inertial fixed frame
+ self.dynComp.setFrameVelocityRepresentation(iDynTree.INERTIAL_FIXED_REPRESENTATION)
+ # print model informations
+ print("The loaded model has", self.dynComp.getNrOfDegreesOfFreedom(), "internal DOFs and", self.dynComp.getNrOfLinks(), "links.")
+ print("The floating base is", self.dynComp.getFloatingBase())
+ # define actual joint configuration and velocity feedbacks
+ self.q_feedback_gazebo = np.zeros(23) # because gazebo model has 4 wheel joints and 4 wheel passive joints
+ self.dq_feedback_gazebo = np.zeros(23)
+ # get number of DOFs and Links
+ self.Dofs = self.dynComp.getNrOfDegreesOfFreedom()
+ self.link_num = self.dynComp.getNrOfLinks()
+ # The gravity acceleration is a 3d acceleration vector.
+ self.gravity = np.asarray([0.0, 0.0, -9.8])
+ # declare floating-base and joint configuration and velocity variables
+ self.rotation_base = iDynTree.Rotation().Identity()
+ self.p_base = np.zeros(3)
+ self.world_T_base = iDynTree.Transform(self.rotation_base, self.p_base)
+ self.s = iDynTree.VectorDynSize(self.Dofs)
+ self.ds = iDynTree.VectorDynSize(self.Dofs)
+ self.base_velocity = iDynTree.Twist(iDynTree.GeomVector3(0, 0, 0), iDynTree.GeomVector3(0, 0, 0))
+ # declare jacobian variables to achieve the right arm ee grasp Jacobian
+ self.Jac_JOINT5_right = iDynTree.MatrixDynSize(6, 6 + self.Dofs)
+ self.Jac_ee_right = np.zeros((6, 6+self.Dofs))
+ self.G_T_JOINT5_right = np.zeros((4, 4))
+ # declare jacobian variables to achieve the left arm ee grasp Jacobian
+ self.Jac_JOINT5_left = iDynTree.MatrixDynSize(6, 6 + self.Dofs)
+ self.Jac_ee_left = np.zeros((6, 6+self.Dofs))
+ self.G_T_JOINT5_left = np.zeros((4, 4))
+ # declare ee and ft position relationship variables
+ self.JOINT5_p_ee_right = np.asarray([-0.08, 0, -0.039-0.04-0.0333])
+ self.JOINT5_p_ee_left = np.asarray([-0.08, 0, -0.039-0.04-0.0333])
+ self.X_transform_right = np.identity(6)
+ self.X_transform_left = np.identity(6)
+ # declare operational-space inertia variables
+ self.G_lambda_r = np.zeros((6, 6))
+ self.G_lambda_l = np.zeros((6, 6))
+ self.G_lambda_r_1DOF = 0
+ self.G_lambda_l_1DOF = 0
+ # declare variables of mass matrix M, centripedal and coriolis force C and gravity force G
+ self.M = iDynTree.MatrixDynSize(self.Dofs + 6, self.Dofs + 6)
+ self.M_inv = np.zeros((self.Dofs + 6, self.Dofs + 6))
+ self.C_G = iDynTree.FreeFloatingGeneralizedTorques(self.model)
+ self.G = iDynTree.FreeFloatingGeneralizedTorques(self.model)
+ # set external torque in the joint space
+ self.linkWrenches = iDynTree.LinkWrenches(self.link_num)
+ self.linkWrenches.zero()
+ self.External_torque = iDynTree.FreeFloatingGeneralizedTorques(self.model)
+ ok = self.dynComp.generalizedExternalForces(self.linkWrenches, self.External_torque)
+
+ def run(self):
+ while not rospy.is_shutdown():
+ # set all robot states
+ self.dynComp.setRobotState(self.world_T_base, self.s, self.base_velocity, self.ds, self.gravity)
+ # calculate two right arm end-effector grasp Jacobian
+ ok = self.dynComp.getFrameFreeFloatingJacobian("RARM_JOINT5_Link", self.Jac_JOINT5_right)
+ self.G_T_JOINT5_right = self.dynComp.getWorldTransform("RARM_JOINT5_Link")
+ self.X_transform_right[0:3, 3:6] = -optas.spatialmath.skew(self.toNumpyArray(self.G_T_JOINT5_right.getRotation(), 3, 3) @ self.JOINT5_p_ee_right)
+ self.Jac_ee_right = self.X_transform_right @ self.Jac_JOINT5_right.toNumPy()
+ # calculate two right arm end-effector grasp Jacobian
+ ok = self.dynComp.getFrameFreeFloatingJacobian("LARM_JOINT5_Link", self.Jac_JOINT5_left)
+ self.G_T_JOINT5_left = self.dynComp.getWorldTransform("LARM_JOINT5_Link")
+ self.X_transform_left[0:3, 3:6] = -optas.spatialmath.skew(self.toNumpyArray(self.G_T_JOINT5_left.getRotation(), 3, 3) @ self.JOINT5_p_ee_left)
+ self.Jac_ee_left = self.X_transform_left @ self.Jac_JOINT5_left.toNumPy()
+ # calculate the mass matrix
+ self.dynComp.getFreeFloatingMassMatrix(self.M)
+ # calculate the centripedal and coriolis force, C, and gravity force G. In this step, it means C+G
+ ok = self.dynComp.generalizedBiasForces(self.C_G)
+ # calculate the gravity force G. In this step, it means G
+ ok = self.dynComp.generalizedGravityForces(self.G)
+ # calculate the inverse of the mass matrix
+ self.M_inv = np.linalg.inv(self.M.toNumPy())
+ # calculate the operational-space inertia of right arm end-effector grasp
+ self.G_lambda_r = np.linalg.inv(self.Jac_ee_right[0:3,:] @ self.M_inv @ np.transpose(self.Jac_ee_right[0:3,:]))
+ self.G_lambda_r_1DOF = 1/(self.Jac_ee_right[1,:] @ self.M_inv @ np.transpose(self.Jac_ee_right[1,:]))
+ # calculate the operational-space inertia of left arm end-effector grasp
+ self.G_lambda_l = np.linalg.inv(self.Jac_ee_left[0:3,:] @ self.M_inv @ np.transpose(self.Jac_ee_left[0:3,:]))
+ self.G_lambda_l_1DOF = 1/(self.Jac_ee_left[1,:] @ self.M_inv @ np.transpose(self.Jac_ee_left[1,:]))
+# a = np.zeros((6, self.Dofs +6))
+# a[0:3, :] = self.Jac_ee_right[0:3,:]
+# a[3:6, :] = self.Jac_ee_left[0:3,:]
+# print(np.linalg.inv(a @ self.M_inv @ np.transpose(a)))
+ # update message
+ self._msg.data = np.array([self.G_lambda_r[0, 0], self.G_lambda_r[0, 1], self.G_lambda_r[0, 2],
+ self.G_lambda_r[1, 1], self.G_lambda_r[1, 2], self.G_lambda_r[2, 2],
+ self.G_lambda_l[0, 0], self.G_lambda_l[0, 1], self.G_lambda_l[0, 2],
+ self.G_lambda_l[1, 1], self.G_lambda_l[1, 2], self.G_lambda_l[2, 2]])
+ self._msg_1DOF.data = np.array([self.G_lambda_r_1DOF, self.G_lambda_l_1DOF])
+ # publish message
+ self._inertia_pub.publish(self._msg)
+ self._inertia_1DOF_pub.publish(self._msg_1DOF)
+ # Jac_donkey = iDynTree.MatrixDynSize(6, 6 + self.Dofs)
+ # ok = self.dynComp.getFrameFreeFloatingJacobian("donkey_link", Jac_donkey)
+ # v= np.zeros(self.Dofs+6)
+ # v[0:6] = self.base_velocity.toNumPy()
+ # v[6:(6+self.Dofs)] = self.ds.toNumPy()
+ # print(v)
+ # print(Jac_donkey)
+ # print(Jac_donkey.toNumPy() @ v)
+ # Sleep for the remaining time to maintain the specified rate
+ self.rate.sleep()
+
+ def read_joint_states_cb(self, msg):
+ self.q_feedback_gazebo = np.asarray(list(msg.position))
+ self.dq_feedback_gazebo = np.asarray(list(msg.velocity))
+ # since idyntree joint number is different with th gazebo.
+ self.s.setVal(0, self.q_feedback_gazebo[0])
+ self.s.setVal(1, self.q_feedback_gazebo[1])
+ self.s.setVal(14, self.q_feedback_gazebo[2])
+ self.s.setVal(2, self.q_feedback_gazebo[3])
+ self.s.setVal(9, self.q_feedback_gazebo[4])
+ self.s.setVal(10, self.q_feedback_gazebo[5])
+ self.s.setVal(11, self.q_feedback_gazebo[6])
+ self.s.setVal(12, self.q_feedback_gazebo[7])
+ self.s.setVal(13, self.q_feedback_gazebo[8])
+ self.s.setVal(3, self.q_feedback_gazebo[9])
+ self.s.setVal(4, self.q_feedback_gazebo[10])
+ self.s.setVal(5, self.q_feedback_gazebo[11])
+ self.s.setVal(6, self.q_feedback_gazebo[12])
+ self.s.setVal(7, self.q_feedback_gazebo[13])
+ self.s.setVal(8, self.q_feedback_gazebo[14])
+ # since idyntree joint number is different with th gazebo.
+ self.ds.setVal(0, self.dq_feedback_gazebo[0])
+ self.ds.setVal(1, self.dq_feedback_gazebo[1])
+ self.ds.setVal(14, self.dq_feedback_gazebo[2])
+ self.ds.setVal(2, self.dq_feedback_gazebo[3])
+ self.ds.setVal(9, self.dq_feedback_gazebo[4])
+ self.ds.setVal(10, self.dq_feedback_gazebo[5])
+ self.ds.setVal(11, self.dq_feedback_gazebo[6])
+ self.ds.setVal(12, self.dq_feedback_gazebo[7])
+ self.ds.setVal(13, self.dq_feedback_gazebo[8])
+ self.ds.setVal(3, self.dq_feedback_gazebo[9])
+ self.ds.setVal(4, self.dq_feedback_gazebo[10])
+ self.ds.setVal(5, self.dq_feedback_gazebo[11])
+ self.ds.setVal(6, self.dq_feedback_gazebo[12])
+ self.ds.setVal(7, self.dq_feedback_gazebo[13])
+ self.ds.setVal(8, self.dq_feedback_gazebo[14])
+
+ def read_base_states_cb(self, msg):
+ self.p_base = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ quaternion = np.array([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.rotation_base = scipy.spatial.transform.Rotation.from_quat(quaternion).as_matrix()
+ self.world_T_base = iDynTree.Transform(self.rotation_base, self.p_base)
+ # sequence: linear and angular
+ self.base_velocity = iDynTree.Twist(iDynTree.GeomVector3(msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z),
+ iDynTree.GeomVector3(msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z))
+
+ def j_X_i(self, j_R_i, i_p_j):
+ """only for idyntree X transformation, because it use linear velocity first and angular velocity second"""
+ X = np.zeros((6, 6))
+ X[0:3, 0:3] = j_R_i @ optas.spatialmath.skew(i_p_j)
+ X[0:3, 3:6] = j_R_i
+ X[3:6, 0:3] = j_R_i
+ X[3:6, 3:6] = np.zeros((3, 3))
+ return X
+
+ def j_X_i(self, j_R_i, j_p_i):
+ """only for idyntree X transformation, because it use linear velocity first and angular velocity second"""
+ X = np.zeros((6, 6))
+ X[0:3, 0:3] = optas.spatialmath.skew(j_p_i) @ j_R_i
+ X[0:3, 3:6] = j_R_i
+ X[3:6, 0:3] = j_R_i
+ X[3:6, 3:6] = np.zeros((3, 3))
+ return X
+
+ def toNumpyArray(self, IdyntreeMatrix, row_num, col_num):
+ NumpyArray = np.zeros((row_num, col_num))
+ for i in range(row_num):
+ for j in range(col_num):
+ NumpyArray[i, j] = IdyntreeMatrix[i, j]
+ return NumpyArray
+
+if __name__ == '__main__':
+ inertia_processor = InertiaProcessing()
+ inertia_processor.run()
diff --git a/script/X_fromRandP.py b/script/X_fromRandP.py
new file mode 100755
index 0000000..17acefb
--- /dev/null
+++ b/script/X_fromRandP.py
@@ -0,0 +1,24 @@
+#! /usr/bin/env python3
+import rospy
+import numpy as np
+import optas
+
+def X_fromRandP(R, p):
+ """ calculate the spatial transformation matrix """
+ X = np.zeros((6,6))
+
+ X[0:3, 0:3] = R
+ X[0:3, 3:6] = np.zeros((3,3))
+ X[3:6, 0:3] = optas.spatialmath.skew(p) @ R
+ X[3:6, 3:6] = R
+ return X
+
+def X_fromRandP_different(R, p):
+ """ calculate the spatial transformation matrix """
+ X = np.zeros((6,6))
+
+ X[0:3, 0:3] = R
+ X[0:3, 3:6] = np.zeros((3,3))
+ X[3:6, 0:3] = -R @ optas.spatialmath.skew(p)
+ X[3:6, 3:6] = R
+ return X
diff --git a/script/action_client_cmd_config.py b/script/action_client_cmd_config.py
new file mode 100755
index 0000000..531e058
--- /dev/null
+++ b/script/action_client_cmd_config.py
@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+import argparse
+from numpy import deg2rad
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdConfigAction, CmdConfigGoal
+
+class CmdConfigClient(object):
+ """docstring for CmdConfigClient."""
+
+ def __init__(self, name, target_config, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_config = target_config # in radians
+ self._duration = duration # in seconds
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = actionlib.SimpleActionClient('/chonk/cmd_config', CmdConfigAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdConfigGoal()
+ goal.positions = self._target_config
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+ done_cb=self.done_cb,
+ active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot joint configuration.')
+# parser.add_argument("--target_config", nargs=23,
+# help="Give target configuration of the robot in degrees.",
+# type=float, default=[0., 0., 0., 0., 0., -20., 0., 0., 0., 0., 0., -20., 0., 0., 0., 1000., 1000., 1000., 1000., 1000., 1000., 1000., 1000.],
+# metavar=('CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5', 'fl_wheel_joint', 'fl_wheel_link_passive_joint', 'fr_wheel_joint', 'fr_wheel_link_passive_joint', 'rl_wheel_joint', 'rl_wheel_link_passive_joint', 'rr_wheel_joint', 'rr_wheel_link_passive_joint')
+# )
+ parser.add_argument("--target_config", nargs=15,
+ help="Give target configuration of the robot in degrees.",
+ type=float, default=[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+ metavar=('CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5' )
+ )
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=5.0)
+ args = vars(parser.parse_args())
+ # get arguments
+ target_config = [deg2rad(q) for q in args['target_config']]
+ duration = args['duration']
+
+ # Initialize node
+ rospy.init_node('cmd_config_client', anonymous=True)
+ # Initialize node class
+ cmd_config_client = CmdConfigClient(rospy.get_name(), target_config, duration)
+ # execute node
+ rospy.spin()
diff --git a/script/action_client_cmd_pose_4tasks.py b/script/action_client_cmd_pose_4tasks.py
new file mode 100755
index 0000000..0770aab
--- /dev/null
+++ b/script/action_client_cmd_pose_4tasks.py
@@ -0,0 +1,145 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+ done_cb=self.done_cb,
+ active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ Donkey_position = [1., 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+# print(Donkey_quaternion.getrpy())
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ right_arm_position_defaut = np.asarray([0.722, -0.1, 0.955])
+ right_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ right_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+# print(optas.spatialmath.rotz(Donkey_angle))
+# print(right_arm_position_defaut.transpose())
+# print(right_arm_position_after_rotation)
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[right_arm_position_after_rotation[0], right_arm_position_after_rotation[1], right_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ right_arm_orientation_default = optas.spatialmath.Quaternion(1., 0., 0., 0.)
+ right_arm_orientation_quaternion = right_arm_orientation_default.__mul__(Donkey_quaternion)
+ right_arm_quaternion = [right_arm_orientation_quaternion.split()[0], right_arm_orientation_quaternion.split()[1], right_arm_orientation_quaternion.split()[2], right_arm_orientation_quaternion.split()[3]]
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=right_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ left_arm_position_defaut = np.asarray([0.722, 0.1, 0.955])
+ left_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ left_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[left_arm_position_after_rotation[0], left_arm_position_after_rotation[1], left_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+# print(optas.spatialmath.rotz(Donkey_angle))
+ left_arm_quaternion = right_arm_quaternion
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=left_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=5.0)
+ args = vars(parser.parse_args())
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_4tasks', anonymous=True)
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient(rospy.get_name(),
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ # execute node
+ rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC.py b/script/action_client_cmd_pose_MPC.py
new file mode 100755
index 0000000..0e6e809
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC.py
@@ -0,0 +1,145 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+ done_cb=self.done_cb,
+ active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [4., 2., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(90.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse right arm arguments
+ right_arm_position_defaut = np.asarray([0.9, -0.12, 0.955])
+ right_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ right_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[right_arm_position_after_rotation[0], right_arm_position_after_rotation[1], right_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ right_arm_orientation_default = optas.spatialmath.Quaternion(1., 0., 0., 0.)
+ right_arm_orientation_quaternion = right_arm_orientation_default.__mul__(Donkey_quaternion)
+ right_arm_quaternion = [right_arm_orientation_quaternion.split()[0], right_arm_orientation_quaternion.split()[1], right_arm_orientation_quaternion.split()[2], right_arm_orientation_quaternion.split()[3]]
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=right_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse left arm arguments
+ left_arm_position_defaut = np.asarray([0.9, 0.12, 0.955])
+ left_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ left_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[left_arm_position_after_rotation[0], left_arm_position_after_rotation[1], left_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ left_arm_quaternion = right_arm_quaternion
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=left_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=5.0)
+ args = vars(parser.parse_args())
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC', anonymous=True)
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient(rospy.get_name(),
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ # execute node
+ rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC.py b/script/action_client_cmd_pose_MPC_BC.py
new file mode 100755
index 0000000..096eba2
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC.py
@@ -0,0 +1,172 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [4., 2., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse right arm arguments
+ right_arm_position_defaut = np.asarray([0.9, -0.12, 0.955])
+ right_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ right_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[right_arm_position_after_rotation[0], right_arm_position_after_rotation[1], right_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ right_arm_orientation_default = optas.spatialmath.Quaternion(1., 0., 0., 0.)
+ right_arm_orientation_quaternion = right_arm_orientation_default.__mul__(Donkey_quaternion)
+ right_arm_quaternion = [right_arm_orientation_quaternion.split()[0], right_arm_orientation_quaternion.split()[1], right_arm_orientation_quaternion.split()[2], right_arm_orientation_quaternion.split()[3]]
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=right_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse left arm arguments
+ left_arm_position_defaut = np.asarray([0.9, 0.12, 0.955])
+ left_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ left_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[left_arm_position_after_rotation[0], left_arm_position_after_rotation[1], left_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ left_arm_quaternion = right_arm_quaternion
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=left_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=5.0)
+ args = vars(parser.parse_args())
+ print(args['target_position_Donkey'])
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC', anonymous=True)
+ client1 = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+# client2 = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+
+# rospy.loginfo("%s: Waiting for action server to start.", 'client1')
+# client1.wait_for_server()
+
+# rospy.loginfo("%s: Waiting for action server to start.", 'client2')
+# client2.wait_for_server()
+
+ # Initialize node class
+ cmd_pose_client1 = CmdPoseClient('client1', client1,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+
+# # Initialize node class
+# cmd_pose_client2 = CmdPoseClient('client2', client2,
+# args['target_position_Donkey'],
+# args['target_orientation_Donkey'],
+# args['target_position_R'],
+# args['target_orientation_R'],
+# args['target_position_L'],
+# args['target_orientation_L'],
+# args['duration']
+# )
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC2.py b/script/action_client_cmd_pose_MPC_BC2.py
new file mode 100755
index 0000000..3ecd260
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC2.py
@@ -0,0 +1,145 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+ done_cb=self.done_cb,
+ active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [5., 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse right arm arguments
+ right_arm_position_defaut = np.asarray([0.9, -0.12, 0.955])
+ right_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ right_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[right_arm_position_after_rotation[0], right_arm_position_after_rotation[1], right_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ right_arm_orientation_default = optas.spatialmath.Quaternion(1., 0., 0., 0.)
+ right_arm_orientation_quaternion = right_arm_orientation_default.__mul__(Donkey_quaternion)
+ right_arm_quaternion = [right_arm_orientation_quaternion.split()[0], right_arm_orientation_quaternion.split()[1], right_arm_orientation_quaternion.split()[2], right_arm_orientation_quaternion.split()[3]]
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=right_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse left arm arguments
+ left_arm_position_defaut = np.asarray([0.9, 0.12, 0.955])
+ left_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ left_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[left_arm_position_after_rotation[0], left_arm_position_after_rotation[1], left_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ left_arm_quaternion = right_arm_quaternion
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=left_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=5.0)
+ args = vars(parser.parse_args())
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC2', anonymous=True)
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient(rospy.get_name(),
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ # execute node
+ rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational.py b/script/action_client_cmd_pose_MPC_BC_operational.py
new file mode 100755
index 0000000..4e84d45
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational.py
@@ -0,0 +1,145 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+ done_cb=self.done_cb,
+ active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [3., 2., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(90.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse right arm arguments
+ right_arm_position_defaut = np.asarray([0.9, -0.12, 0.955])
+ right_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ right_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[right_arm_position_after_rotation[0], right_arm_position_after_rotation[1], right_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ right_arm_orientation_default = optas.spatialmath.Quaternion(1., 0., 0., 0.)
+ right_arm_orientation_quaternion = right_arm_orientation_default.__mul__(Donkey_quaternion)
+ right_arm_quaternion = [right_arm_orientation_quaternion.split()[0], right_arm_orientation_quaternion.split()[1], right_arm_orientation_quaternion.split()[2], right_arm_orientation_quaternion.split()[3]]
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=right_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse left arm arguments
+ left_arm_position_defaut = np.asarray([0.9, 0.12, 0.955])
+ left_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ left_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[left_arm_position_after_rotation[0], left_arm_position_after_rotation[1], left_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ left_arm_quaternion = right_arm_quaternion
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=left_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=5.0)
+ args = vars(parser.parse_args())
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational', anonymous=True)
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient(rospy.get_name(),
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ # execute node
+ rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_AD_obstable_pick.py b/script/action_client_cmd_pose_MPC_BC_operational_AD_obstable_pick.py
new file mode 100755
index 0000000..8d0a602
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_AD_obstable_pick.py
@@ -0,0 +1,413 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.83, 2-0.25, 1.15]
+ args['target_position_L'] = [3.83, 2+0.25, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.83, 2-0.15, 1.15]
+ args['target_position_L'] = [3.83, 2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, 2-0.15, 1.15]
+ args['target_position_L'] = [3.83, 2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, 2-0.15, 1.25]
+ args['target_position_L'] = [3.83, 2+0.15, 1.25]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.25]
+ args['target_position_L'] = [3.83, -2+0.15, 1.25]
+
+ # Initialize node class
+ args['duration']=20.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.15]
+ args['target_position_L'] = [3.83, -2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.15]
+ args['target_position_L'] = [3.83, -2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.83, -2-0.24, 1.15]
+ args['target_position_L'] = [3.83, -2+0.24, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.24, 1.15]
+ args['target_position_L'] = [2, -2+0.24, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=1.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_AD_obstable_pick_longhorizon.py b/script/action_client_cmd_pose_MPC_BC_operational_AD_obstable_pick_longhorizon.py
new file mode 100755
index 0000000..6693f09
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_AD_obstable_pick_longhorizon.py
@@ -0,0 +1,413 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.83, 2-0.25, 1.15]
+ args['target_position_L'] = [3.83, 2+0.25, 1.15]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.83, 2-0.15, 1.15]
+ args['target_position_L'] = [3.83, 2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, 2-0.15, 1.15]
+ args['target_position_L'] = [3.83, 2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, 2-0.15, 1.25]
+ args['target_position_L'] = [3.83, 2+0.15, 1.25]
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.25]
+ args['target_position_L'] = [3.83, -2+0.15, 1.25]
+
+ # Initialize node class
+ args['duration']=12.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.15]
+ args['target_position_L'] = [3.83, -2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.15]
+ args['target_position_L'] = [3.83, -2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.83, -2-0.24, 1.15]
+ args['target_position_L'] = [3.83, -2+0.24, 1.15]
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.24, 1.15]
+ args['target_position_L'] = [2, -2+0.24, 1.15]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=1.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_AD_pick.py b/script/action_client_cmd_pose_MPC_BC_operational_AD_pick.py
new file mode 100755
index 0000000..1a30784
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_AD_pick.py
@@ -0,0 +1,413 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.83, 2-0.25, 1.15]
+ args['target_position_L'] = [3.83, 2+0.25, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.83, 2-0.15, 1.15]
+ args['target_position_L'] = [3.83, 2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 10, 0]
+ args['target_force_L'] = [0, -10, 0]
+ args['target_position_R'] = [3.83, 2-0.15, 1.15]
+ args['target_position_L'] = [3.83, 2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 10, 0]
+ args['target_force_L'] = [0, -10, 0]
+ args['target_position_R'] = [3.83, 2-0.15, 1.25]
+ args['target_position_L'] = [3.83, 2+0.15, 1.25]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 10, 0]
+ args['target_force_L'] = [0, -10, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.25]
+ args['target_position_L'] = [3.83, -2+0.15, 1.25]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 10, 0]
+ args['target_force_L'] = [0, -10, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.15]
+ args['target_position_L'] = [3.83, -2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.15]
+ args['target_position_L'] = [3.83, -2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.83, -2-0.24, 1.15]
+ args['target_position_L'] = [3.83, -2+0.24, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.24, 1.15]
+ args['target_position_L'] = [2, -2+0.24, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=1.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_TC.py b/script/action_client_cmd_pose_MPC_BC_operational_TC.py
new file mode 100755
index 0000000..5bd1fd3
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_TC.py
@@ -0,0 +1,277 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.85, 2-0.25, 1.15]
+ args['target_position_L'] = [3.85, 2+0.25, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.85, 2-0.13, 1.15]
+ args['target_position_L'] = [3.85, 2+0.13, 1.15]
+ args['duration']=5
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.85, 2-0.13, 1.25]
+ args['target_position_L'] = [3.85, 2+0.13, 1.25]
+ args['duration']=4
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.85, -2-0.13, 1.25]
+ args['target_position_L'] = [3.85, -2+0.13, 1.25]
+ args['duration']=7
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.85, -2-0.13, 1.15]
+ args['target_position_L'] = [3.85, -2+0.13, 1.15]
+ args['duration']=4
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.85, -2-0.24, 1.15]
+ args['target_position_L'] = [3.85, -2+0.24, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.24, 1.15]
+ args['target_position_L'] = [2, -2+0.24, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_obstable_sensorAD_notGood.py b/script/action_client_cmd_pose_MPC_BC_operational_obstable_sensorAD_notGood.py
new file mode 100755
index 0000000..08631f3
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_obstable_sensorAD_notGood.py
@@ -0,0 +1,413 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.83, 2-0.25, 1.15]
+ args['target_position_L'] = [3.83, 2+0.25, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.83, 2-0.15, 1.15]
+ args['target_position_L'] = [3.83, 2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, 2-0.15, 1.15]
+ args['target_position_L'] = [3.83, 2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, 2-0.15, 1.25]
+ args['target_position_L'] = [3.83, 2+0.15, 1.25]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.25]
+ args['target_position_L'] = [3.83, -2+0.15, 1.25]
+
+ # Initialize node class
+ args['duration']=20.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 18, 0]
+ args['target_force_L'] = [0, -18, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.15]
+ args['target_position_L'] = [3.83, -2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.83, -2-0.15, 1.15]
+ args['target_position_L'] = [3.83, -2+0.15, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.83, -2-0.24, 1.15]
+ args['target_position_L'] = [3.83, -2+0.24, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.24, 1.15]
+ args['target_position_L'] = [2, -2+0.24, 1.15]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick.py b/script/action_client_cmd_pose_MPC_BC_operational_pick.py
new file mode 100755
index 0000000..7c3eeb1
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick.py
@@ -0,0 +1,277 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.8, 2-0.25, 1.15]
+ args['target_position_L'] = [3.8, 2+0.25, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.8, 2-0.135, 1.15]
+ args['target_position_L'] = [3.8, 2+0.135, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.8, 2-0.135, 1.25]
+ args['target_position_L'] = [3.8, 2+0.135, 1.25]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.8, -2-0.135, 1.25]
+ args['target_position_L'] = [3.8, -2+0.135, 1.25]
+ args['duration']=5
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.8, -2-0.135, 1.15]
+ args['target_position_L'] = [3.8, -2+0.135, 1.15]
+ args['duration']=4
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.8, -2-0.24, 1.15]
+ args['target_position_L'] = [3.8, -2+0.24, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.24, 1.15]
+ args['target_position_L'] = [2, -2+0.24, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_experiment.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_experiment.py
new file mode 100755
index 0000000..7741df3
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_experiment.py
@@ -0,0 +1,460 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.25+0.3, 4, 1.0],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.25-0.3, 4, 1.0],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=12.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+# delta_x = 0.025+0.016
+
+ args['target_position_R'] = [2.25+0.3, 4.38, 1.0]
+ args['target_position_L'] = [2.25-0.3, 4.38, 1.0]
+ args['duration']=3
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065, 4.38, 1.0]
+ args['target_position_L'] = [2.25-0.3+0.065, 4.38, 1.0]
+ args['duration']=3
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [2.25+0.3-0.065, 4.38, 1.1]
+ args['target_position_L'] = [2.25-0.3+0.065, 4.38, 1.1]
+ args['duration']=3
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065, 4.38-1.0, 1.1]
+ args['target_position_L'] = [2.25-0.3+0.065, 4.38-1.0, 1.1]
+ args['duration']=5
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065-2.4, 4.38-1.0, 1.1]
+ args['target_position_L'] = [2.25-0.3+0.065-2.4, 4.38-1.0, 1.1]
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065-2.4, 4.38-0.2, 1.1]
+ args['target_position_L'] = [2.25-0.3+0.065-2.4, 4.38-0.2, 1.1]
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065-2.4, 4.38-0.2, 0.9]
+ args['target_position_L'] = [2.25-0.3+0.065-2.4, 4.38-0.2, 0.9]
+ args['duration']=7
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065-2.4+0.1, 4.38-0.2, 0.9]
+ args['target_position_L'] = [2.25-0.3+0.065-2.4-0.1, 4.38-0.2, 0.9]
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065-2.4+0.1, 4.38-0.6, 0.9]
+ args['target_position_L'] = [2.25-0.3+0.065-2.4-0.1, 4.38-0.6, 0.9]
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.15, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.15, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+ args['duration']=11
+
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+###########################################################################################################
+
+ args['target_position_R'] = [0.865, -0.15, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.15, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+ args['duration']=1
+
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+ args['target_position_R'] = [2.25+0.3-0.065-2.4+0.1, 4.38-0.6, 0.92]
+ args['target_position_L'] = [2.25-0.3+0.065-2.4-0.1, 4.38-0.6, 0.92]
+ args['duration']=11
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065-2.4+0.1, 4.38-0.2, 0.92]
+ args['target_position_L'] = [2.25-0.3+0.065-2.4-0.1, 4.38-0.2, 0.92]
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065-2.4, 4.38-0.2, 0.92]
+ args['target_position_L'] = [2.25-0.3+0.065-2.4, 4.38-0.2, 0.92]
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065-2.4, 4.38-0.2, 1.1]
+ args['target_position_L'] = [2.25-0.3+0.065-2.4, 4.38-0.2, 1.1]
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065-2.4, 4.38-1.0, 1.1]
+ args['target_position_L'] = [2.25-0.3+0.065-2.4, 4.38-1.0, 1.1]
+ args['duration']=5
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065, 4.38-1.0, 1.1]
+ args['target_position_L'] = [2.25-0.3+0.065, 4.38-1.0, 1.1]
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3-0.065, 4.38, 1.1]
+ args['target_position_L'] = [2.25-0.3+0.065, 4.38, 1.1]
+ args['duration']=5
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+
+
+ args['target_position_R'] = [2.25+0.3-0.065, 4.38, 1.02]
+ args['target_position_L'] = [2.25-0.3+0.065, 4.38, 1.02]
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3, 4.38, 1.02]
+ args['target_position_L'] = [2.25-0.3, 4.38, 1.02]
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2.25+0.3, 4.38-0.3, 1.0]
+ args['target_position_L'] = [2.25-0.3, 4.38-0.3, 1.0]
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.15, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.15, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+ args['duration']=11
+
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+
+
+
+
+
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_experiment_bigbox.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_experiment_bigbox.py
new file mode 100755
index 0000000..0e15df3
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_experiment_bigbox.py
@@ -0,0 +1,418 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+
+ head_pub = rospy.Publisher("/chonk/head_states", Float64MultiArray, queue_size=10)
+
+ _msg = Float64MultiArray()
+ _msg.layout = MultiArrayLayout()
+ _msg.layout.data_offset = 0
+ _msg.layout.dim.append(MultiArrayDimension())
+ _msg.layout.dim[0].label = "columns"
+ _msg.layout.dim[0].size = 2
+
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[1.45, 2.6, 1.0],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[1.45, 3.2, 1.0],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=12.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+
+ head_pos1 = -0.15
+ head_pos2 = 0.25
+# head_pos2 = -0.15
+
+
+ _msg.data = np.array([head_pos1, head_pos1])
+ head_pub.publish(_msg)
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ _msg.data = np.array([head_pos1, head_pos2])
+ head_pub.publish(_msg)
+
+ args['target_position_R'] = [1.45+0.6, 2.6, 1.0]
+ args['target_position_L'] = [1.45+0.6, 3.2, 1.0]
+ args['duration']=5
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+
+ _msg.data = np.array([head_pos2, head_pos2])
+ head_pub.publish(_msg)
+
+
+ args['target_position_R'] = [1.45+0.6, 2.6+0.06, 1.0]
+ args['target_position_L'] = [1.45+0.6, 3.2-0.06, 1.0]
+ args['duration']=3
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ _msg.data = np.array([head_pos2, head_pos2])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [1.45+0.6, 2.6+0.06, 1.1]
+ args['target_position_L'] = [1.45+0.6, 3.2-0.06, 1.1]
+ args['duration']=3
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ _msg.data = np.array([head_pos2, head_pos2])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [1.45+0.6, 2.6+0.06-2, 1.1]
+ args['target_position_L'] = [1.45+0.6, 3.2-0.06-2, 1.1]
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+
+ _msg.data = np.array([head_pos2, head_pos2])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [1.45+0.6+5.8, 2.6+0.06-2, 1.1]
+ args['target_position_L'] = [1.45+0.6+5.8, 3.2-0.06-2, 1.1]
+ args['duration']=15
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+
+ _msg.data = np.array([head_pos2, head_pos2])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [1.45+0.6+5.8, 2.6+0.06-2, 0.9]
+ args['target_position_L'] = [1.45+0.6+5.8, 3.2-0.06-2, 0.9]
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ _msg.data = np.array([head_pos2, head_pos1])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [1.45+0.6+5.8, 2.6-2, 0.9]
+ args['target_position_L'] = [1.45+0.6+5.8, 3.2-2, 0.9]
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ _msg.data = np.array([head_pos1, head_pos1])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [1.45+5.8, 2.6-2, 0.9]
+ args['target_position_L'] = [1.45+5.8, 3.2-2, 0.9]
+ args['duration']=10
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ _msg.data = np.array([head_pos1, head_pos1])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [7.25-1+0.2, 2.5, 0.9]
+ args['target_position_L'] = [7.25-1-0.2, 2.5, 0.9]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2 , np.pi/2]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2 , np.pi/2]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ _msg.data = np.array([head_pos1, head_pos1])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [7.25-2, 3.1-2, 0.9]
+ args['target_position_L'] = [7.25-2, 2.7-2, 0.9]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2 , np.pi]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2 , np.pi]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ _msg.data = np.array([head_pos1, head_pos1])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [7.25-6, 3.1-2, 0.9]
+ args['target_position_L'] = [7.25-6, 2.7-2, 0.9]
+ args['duration']=10
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ _msg.data = np.array([head_pos1, head_pos1])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [7.25-6+0.2, 2.5, 0.9]
+ args['target_position_L'] = [7.25-6-0.2, 2.5, 0.9]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2 , np.pi/2]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2 , np.pi/2]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ _msg.data = np.array([head_pos1, head_pos1])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [0.865+0.2, -0.15, 0.9]
+ args['target_position_L'] = [0.865+0.2, 0.15, 0.9]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2 , 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2 , 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+
+
+# _msg.data = np.array([head_pos2, head_pos1])
+# head_pub.publish(_msg)
+# args['target_position_R'] = [2.25+0.3-0.06-2.4+0.1, 4.38-1.0, 1.03]
+# args['target_position_L'] = [2.25-0.3+0.06-2.4-0.1, 4.38-1.0, 1.03]
+# args['duration']=4
+# cmd_pose_client = CmdPoseClient('client', client,
+# args['target_position_Donkey'],
+# args['target_orientation_Donkey'],
+# args['target_position_R'],
+# args['target_orientation_R'],
+# args['target_position_L'],
+# args['target_orientation_L'],
+# args['duration']
+# )
+
+ _msg.data = np.array([head_pos1, head_pos1])
+ head_pub.publish(_msg)
+ args['target_position_R'] = [0.865, -0.15, 0.9]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.15, 0.9]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+ args['duration']=3
+
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD.py
new file mode 100755
index 0000000..3e04e32
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD.py
@@ -0,0 +1,435 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 30
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.85, 2-0.22, 1.165]
+ args['target_position_L'] = [3.85, 2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.85, 2-0.142, 1.165]
+ args['target_position_L'] = [3.85, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, 2-0.142, 1.165]
+ args['target_position_L'] = [3.85, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, 2-0.142, 1.25]
+ args['target_position_L'] = [3.85, 2+0.142, 1.25]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.25]
+ args['target_position_L'] = [3.85, -2+0.142, 1.25]
+
+ # Initialize node class
+ args['duration']=10
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ args['m_box'] = 0 # unit kg
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.22, 1.165]
+ args['target_position_L'] = [3.85, -2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.22, 1.165]
+ args['target_position_L'] = [2, -2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=1.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.py
new file mode 100755
index 0000000..8bd541f
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.py
@@ -0,0 +1,435 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 30
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.8, 2-0.22, 1.165]
+ args['target_position_L'] = [3.8, 2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.8, 2-0.142, 1.165]
+ args['target_position_L'] = [3.8, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.8, 2-0.142, 1.165]
+ args['target_position_L'] = [3.8, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.8, 2-0.142, 1.25]
+ args['target_position_L'] = [3.8, 2+0.142, 1.25]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.8, -2-0.142, 1.25]
+ args['target_position_L'] = [3.8, -2+0.142, 1.25]
+
+ # Initialize node class
+ args['duration']=10
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.8, -2-0.142, 1.165]
+ args['target_position_L'] = [3.8, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ args['m_box'] = 0 # unit kg
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.8, -2-0.142, 1.165]
+ args['target_position_L'] = [3.8, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.8, -2-0.22, 1.165]
+ args['target_position_L'] = [3.8, -2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.22, 1.165]
+ args['target_position_L'] = [2, -2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=1.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon_turn.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon_turn.py
new file mode 100755
index 0000000..4d5a23d
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon_turn.py
@@ -0,0 +1,415 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.5+0.22, 2.5, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.5-0.22, 2.5, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=8.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 15
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=10.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [2.5+0.22, 3, 1.165]
+ args['target_position_L'] = [2.5-0.22, 3, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [2.5+0.142, 3, 1.165]
+ args['target_position_L'] = [2.5-0.142, 3, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [2.5+0.142, 3, 1.165]
+ args['target_position_L'] = [2.5-0.142, 3, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [2.5+0.142, 3, 1.25]
+ args['target_position_L'] = [2.5-0.142, 3, 1.25]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.25]
+ args['target_position_L'] = [3.85, -2+0.142, 1.25]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=14
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+ # Initialize node class
+ args['duration']=3
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.22, 1.165]
+ args['target_position_L'] = [3.85, -2+0.22, 1.165]
+ # Initialize node class
+ args['duration']=3
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [2, -2-0.22, 1.165]
+ args['target_position_L'] = [2, -2+0.22, 1.165]
+ # Initialize node class
+ args['duration']=5
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.py
new file mode 100755
index 0000000..0dc4110
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.py
@@ -0,0 +1,434 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 50
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.85, 2-0.22, 1.165]
+ args['target_position_L'] = [3.85, 2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.85, 2-0.142, 1.165]
+ args['target_position_L'] = [3.85, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, 2-0.142, 1.165]
+ args['target_position_L'] = [3.85, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, 2-0.142, 1.27]
+ args['target_position_L'] = [3.85, 2+0.142, 1.27]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.27]
+ args['target_position_L'] = [3.85, -2+0.142, 1.27]
+
+ # Initialize node class
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ args['m_box'] = 0 # unit kg
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.25, 1.165]
+ args['target_position_L'] = [3.85, -2+0.25, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.22, 1.165]
+ args['target_position_L'] = [2, -2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn.py
new file mode 100755
index 0000000..7e62385
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn.py
@@ -0,0 +1,420 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(0.)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.5+0.18, 1.5, 1.065],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 , np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.5-0.18, 1.5, 1.065],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 , np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=8.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 30
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=8.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ args['target_position_R'] = [2.5+0.22, 2.95, 1.165]
+ args['target_position_L'] = [2.5-0.22, 2.95, 1.165]
+
+ # Initialize node class
+ args['duration']=6.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [2.5+0.142, 2.95, 1.165]
+ args['target_position_L'] = [2.5-0.142, 2.95, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [2.5+0.142, 2.95, 1.165]
+ args['target_position_L'] = [2.5-0.142, 2.95, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [2.5+0.142, 2.95, 1.25]
+ args['target_position_L'] = [2.5-0.142, 2.95, 1.25]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.25]
+ args['target_position_L'] = [3.85, -2+0.142, 1.25]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=12
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+ # Initialize node class
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.22, 1.165]
+ args['target_position_L'] = [3.85, -2+0.22, 1.165]
+ # Initialize node class
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [2, -2-0.25, 1.165]
+ args['target_position_L'] = [2, -2+0.25, 1.165]
+ # Initialize node class
+ args['duration']=5
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn_experiment.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn_experiment.py
new file mode 100755
index 0000000..9415e02
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_turn_experiment.py
@@ -0,0 +1,427 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ self.tf_listener = tf.TransformListener()
+ # read current robot joint positions
+ try:
+ (trans_box,rot_box) = self.tf_listener.lookupTransform('/vicon/world', 'vicon/chonk/BOX', rospy.Time(0))
+ except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+ print("error: cannot find vicon data!!!!")
+
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(0.)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.5+0.18, 1.5, 1.065],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 , np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.5-0.18, 1.5, 1.065],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 , np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=8.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 30
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=8.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ args['target_position_R'] = [2.5+0.22, 2.95, 1.165]
+ args['target_position_L'] = [2.5-0.22, 2.95, 1.165]
+
+ # Initialize node class
+ args['duration']=6.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [2.5+0.142, 2.95, 1.165]
+ args['target_position_L'] = [2.5-0.142, 2.95, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [2.5+0.142, 2.95, 1.165]
+ args['target_position_L'] = [2.5-0.142, 2.95, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [2.5+0.142, 2.95, 1.25]
+ args['target_position_L'] = [2.5-0.142, 2.95, 1.25]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.25]
+ args['target_position_L'] = [3.85, -2+0.142, 1.25]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=12
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+ # Initialize node class
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.22, 1.165]
+ args['target_position_L'] = [3.85, -2+0.22, 1.165]
+ # Initialize node class
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [2, -2-0.25, 1.165]
+ args['target_position_L'] = [2, -2+0.25, 1.165]
+ # Initialize node class
+ args['duration']=5
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py
new file mode 100755
index 0000000..8a05537
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py
@@ -0,0 +1,433 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 30
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=6.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ args['target_position_R'] = [3.85, 2-0.22, 1.165]
+ args['target_position_L'] = [3.85, 2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=6.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.85, 2-0.145, 1.165]
+ args['target_position_L'] = [3.85, 2+0.145, 1.165]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, 2-0.145, 1.165]
+ args['target_position_L'] = [3.85, 2+0.145, 1.165]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, 2-0.145, 1.31]
+ args['target_position_L'] = [3.85, 2+0.145, 1.31]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.145, 1.31]
+ args['target_position_L'] = [3.85, -2+0.145, 1.31]
+
+ # Initialize node class
+ args['duration']=12
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.145, 1.165]
+ args['target_position_L'] = [3.85, -2+0.145, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ args['m_box'] = 0 # unit kg
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.145, 1.165]
+ args['target_position_L'] = [3.85, -2+0.145, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.25, 1.165]
+ args['target_position_L'] = [3.85, -2+0.25, 1.165]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.25, 1.165]
+ args['target_position_L'] = [2, -2+0.25, 1.165]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn.py
new file mode 100755
index 0000000..6330b95
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation_turn.py
@@ -0,0 +1,469 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(0.)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.5+0.18, 2, 1.065],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 , np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.5-0.18, 2, 1.065],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 , np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=8.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 30
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=8.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ args['target_position_R'] = [2.5+0.22, 3, 1.165]
+ args['target_position_L'] = [2.5-0.22, 3, 1.165]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [2.5+0.145, 3, 1.165]
+ args['target_position_L'] = [2.5-0.145, 3, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [2.5+0.145, 3, 1.165]
+ args['target_position_L'] = [2.5-0.145, 3, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [2.5+0.145, 3, 1.31]
+ args['target_position_L'] = [2.5-0.145, 3, 1.31]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [2.5+0.145, 2.8, 1.31]
+ args['target_position_L'] = [2.5-0.145, 2.8, 1.31]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3, 2.2-0.145, 1.31]
+ args['target_position_L'] = [3, 2.2+0.145, 1.31]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.145, 1.31]
+ args['target_position_L'] = [3.85, -2+0.145, 1.31]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=12
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.145, 1.165]
+ args['target_position_L'] = [3.85, -2+0.145, 1.165]
+ # Initialize node class
+ args['duration']=4
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.25, 1.165]
+ args['target_position_L'] = [3.85, -2+0.25, 1.165]
+ # Initialize node class
+ args['duration']=5
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [2, -2-0.25, 1.165]
+ args['target_position_L'] = [2, -2+0.25, 1.165]
+ # Initialize node class
+ args['duration']=5
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_turn.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_turn.py
new file mode 100755
index 0000000..20664d5
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_localsensorAD_turn.py
@@ -0,0 +1,415 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.5+0.22, 3, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2.5-0.22, 3, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, np.pi/2]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=8.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 15
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=10.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [2.5+0.22, 3.5, 1.165]
+ args['target_position_L'] = [2.5-0.22, 3.5, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [2.5+0.142, 3.5, 1.165]
+ args['target_position_L'] = [2.5-0.142, 3.5, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [2.5+0.142, 3.5, 1.165]
+ args['target_position_L'] = [2.5-0.142, 3.5, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [2.5+0.142, 3.5, 1.25]
+ args['target_position_L'] = [2.5-0.142, 3.5, 1.25]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.25]
+ args['target_position_L'] = [3.85, -2+0.142, 1.25]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=10
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, force]
+ args['target_force_L'] = [0, 0, force]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+ # Initialize node class
+ args['duration']=3
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.22, 1.165]
+ args['target_position_L'] = [3.85, -2+0.22, 1.165]
+ # Initialize node class
+ args['duration']=3
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [2, -2-0.22, 1.165]
+ args['target_position_L'] = [2, -2+0.22, 1.165]
+ # Initialize node class
+ args['duration']=5
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=10.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD.py
new file mode 100755
index 0000000..c8d9bf9
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD.py
@@ -0,0 +1,435 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 30
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.85, 2-0.22, 1.165]
+ args['target_position_L'] = [3.85, 2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.85, 2-0.142, 1.165]
+ args['target_position_L'] = [3.85, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, 2-0.142, 1.165]
+ args['target_position_L'] = [3.85, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, 2-0.142, 1.25]
+ args['target_position_L'] = [3.85, 2+0.142, 1.25]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, -2-0.142, 1.25]
+ args['target_position_L'] = [3.85, -2+0.142, 1.25]
+
+ # Initialize node class
+ args['duration']=10
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ args['m_box'] = 0 # unit kg
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.22, 1.165]
+ args['target_position_L'] = [3.85, -2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.22, 1.165]
+ args['target_position_L'] = [2, -2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=1.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.py
new file mode 100755
index 0000000..d481459
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.py
@@ -0,0 +1,435 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+# rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 50
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.8, 2-0.22, 1.165]
+ args['target_position_L'] = [3.8, 2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.8, 2-0.142, 1.165]
+ args['target_position_L'] = [3.8, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.8, 2-0.142, 1.165]
+ args['target_position_L'] = [3.8, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.8, 2-0.142, 1.25]
+ args['target_position_L'] = [3.8, 2+0.142, 1.25]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.8, -2-0.142, 1.25]
+ args['target_position_L'] = [3.8, -2+0.142, 1.25]
+
+ # Initialize node class
+ args['duration']=10
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.8, -2-0.142, 1.165]
+ args['target_position_L'] = [3.8, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ args['m_box'] = 0 # unit kg
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.8, -2-0.142, 1.165]
+ args['target_position_L'] = [3.8, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.8, -2-0.22, 1.165]
+ args['target_position_L'] = [3.8, -2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.22, 1.165]
+ args['target_position_L'] = [2, -2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=1.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.py
new file mode 100755
index 0000000..b93c7a4
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.py
@@ -0,0 +1,434 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 50
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.85, 2-0.22, 1.165]
+ args['target_position_L'] = [3.85, 2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.85, 2-0.142, 1.165]
+ args['target_position_L'] = [3.85, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, 2-0.142, 1.165]
+ args['target_position_L'] = [3.85, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, 2-0.142, 1.27]
+ args['target_position_L'] = [3.85, 2+0.142, 1.27]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, -2-0.142, 1.27]
+ args['target_position_L'] = [3.85, -2+0.142, 1.27]
+
+ # Initialize node class
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ args['m_box'] = 0 # unit kg
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.25, 1.165]
+ args['target_position_L'] = [3.85, -2+0.25, 1.165]
+
+ # Initialize node class
+ args['duration']=3.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.22, 1.165]
+ args['target_position_L'] = [2, -2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=1.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+
diff --git a/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py b/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py
new file mode 100755
index 0000000..dee505b
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py
@@ -0,0 +1,433 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, m_box, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_force_R, target_torque_R,
+ target_pos_L, target_quat_L, target_force_L, target_torque_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._m_box = m_box
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_force_R = target_force_R
+ self._target_torque_R = target_torque_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._target_force_L = target_force_L
+ self._target_torque_L = target_torque_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseForceGoal()
+ goal.m_box = self._m_box
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.ForceTorqueR.force.x = self._target_force_R[0]
+ goal.ForceTorqueR.force.y = self._target_force_R[1]
+ goal.ForceTorqueR.force.z = self._target_force_R[2]
+ goal.ForceTorqueR.torque.x = self._target_torque_R[0]
+ goal.ForceTorqueR.torque.y = self._target_torque_R[1]
+ goal.ForceTorqueR.torque.z = self._target_torque_R[2]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.ForceTorqueL.force.x = self._target_force_L[0]
+ goal.ForceTorqueL.force.y = self._target_force_L[1]
+ goal.ForceTorqueL.force.z = self._target_force_L[2]
+ goal.ForceTorqueL.torque.x = self._target_torque_L[0]
+ goal.ForceTorqueL.torque.y = self._target_torque_L[1]
+ goal.ForceTorqueL.torque.z = self._target_torque_L[2]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+ pass
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ parser.add_argument("--m_box", help="Give box mass.", type=float, default=0.0)
+
+
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_R', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.22, 1.165],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3-0.2, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_force_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0, 0, 0],
+ metavar=('force_X', 'force_Y', 'force_Z')
+ )
+ parser.add_argument('--target_torque_L', nargs=3,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[0, 0, 0],
+ metavar=('torque_X','torque_Y','torque_Z')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_operational_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseForceAction)
+
+ force = 50
+ m_box = 1.2
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ args['target_position_R'] = [3.85, 2-0.22, 1.165]
+ args['target_position_L'] = [3.85, 2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+ args['target_position_R'] = [3.85, 2-0.142, 1.165]
+ args['target_position_L'] = [3.85, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['m_box'] = m_box # unit kg
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, 2-0.142, 1.165]
+ args['target_position_L'] = [3.85, 2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, 2-0.142, 1.27]
+ args['target_position_L'] = [3.85, 2+0.142, 1.27]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, -2-0.142, 1.27]
+ args['target_position_L'] = [3.85, -2+0.142, 1.27]
+
+ # Initialize node class
+ args['duration']=8
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, force, 0]
+ args['target_force_L'] = [0, -force, 0]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+ args['m_box'] = 0 # unit kg
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.142, 1.165]
+ args['target_position_L'] = [3.85, -2+0.142, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_force_R'] = [0, 0, 0]
+ args['target_force_L'] = [0, 0, 0]
+ args['target_position_R'] = [3.85, -2-0.22, 1.165]
+ args['target_position_L'] = [3.85, -2+0.22, 1.165]
+
+ # Initialize node class
+ args['duration']=4.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.25, 1.165]
+ args['target_position_L'] = [2, -2+0.25, 1.165]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi, np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ args['duration']=5.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+ # Initialize node class
+ args['duration']=2.0
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['m_box'],
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_force_R'],
+ args['target_torque_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['target_force_L'],
+ args['target_torque_L'],
+ args['duration']
+ )
+
+
+
diff --git a/script/action_client_cmd_pose_MPC_BC_pick.py b/script/action_client_cmd_pose_MPC_BC_pick.py
new file mode 100755
index 0000000..b8fb32f
--- /dev/null
+++ b/script/action_client_cmd_pose_MPC_BC_pick.py
@@ -0,0 +1,265 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, client, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = client
+# self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+# done_cb=self.done_cb,
+# active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [0, 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2-0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ ori_R = optas.spatialmath.Quaternion.fromrpy([np.pi+np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_R[0], ori_R[1], ori_R[2], ori_R[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse left arm arguments
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[2, 2+0.25, 1.15],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+
+ ori_L = optas.spatialmath.Quaternion.fromrpy([np.pi-np.pi/2, np.pi/2 - np.pi/3, 0]).getquat()
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[ori_L[0], ori_L[1], ori_L[2], ori_L[3]],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=4.0)
+ args = vars(parser.parse_args())
+
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_MPC_BC_pick', anonymous=True)
+ client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ # execute node
+# rospy.spin()
+
+ args['target_position_R'] = [3.8, 2-0.25, 1.15]
+ args['target_position_L'] = [3.8, 2+0.25, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.8, 2-0.148, 1.15]
+ args['target_position_L'] = [3.8, 2+0.148, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.8, 2-0.148, 1.25]
+ args['target_position_L'] = [3.8, 2+0.148, 1.25]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.8, -2-0.148, 1.25]
+ args['target_position_L'] = [3.8, -2+0.148, 1.25]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.8, -2-0.148, 1.15]
+ args['target_position_L'] = [3.8, -2+0.148, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [3.8, -2-0.24, 1.15]
+ args['target_position_L'] = [3.8, -2+0.24, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [2, -2-0.24, 1.15]
+ args['target_position_L'] = [2, -2+0.24, 1.15]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+ args['target_position_R'] = [0.865, -0.12, 0.92]
+ ori_R = optas.spatialmath.Quaternion.fromrpy([0, -np.pi/2, 0]).getquat()
+ args['target_orientation_R'] = [ori_R[0], ori_R[1], ori_R[2], ori_R[3]]
+ args['target_position_L'] = [0.865, 0.12, 0.92]
+ ori_L = optas.spatialmath.Quaternion.fromrpy([0, -np.pi/2, 0]).getquat()
+ args['target_orientation_L'] = [ori_L[0], ori_L[1], ori_L[2], ori_L[3]]
+
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient('client', client,
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+
+
+ # execute node
+# rospy.spin()
diff --git a/script/action_client_cmd_pose_fixedbase.py b/script/action_client_cmd_pose_fixedbase.py
new file mode 100755
index 0000000..3143fda
--- /dev/null
+++ b/script/action_client_cmd_pose_fixedbase.py
@@ -0,0 +1,102 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdDualPoseAction, CmdDualPoseGoal
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdDualPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdDualPoseGoal()
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+ done_cb=self.done_cb,
+ active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot end-effector pose.')
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0.722, -0.1, 0.955],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[1., 0., 0., 0.],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[0.722, 0.1, 0.955],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[1., 0., 0., 0.],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=5.0)
+ args = vars(parser.parse_args())
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client', anonymous=True)
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient(rospy.get_name(),
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ # execute node
+ rospy.spin()
diff --git a/script/action_client_cmd_pose_planner.py b/script/action_client_cmd_pose_planner.py
new file mode 100755
index 0000000..6fe6492
--- /dev/null
+++ b/script/action_client_cmd_pose_planner.py
@@ -0,0 +1,145 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+ done_cb=self.done_cb,
+ active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [1., 0., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(0.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse right arm arguments
+ right_arm_position_defaut = np.asarray([0.9, -0.12, 0.955])
+ right_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ right_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[right_arm_position_after_rotation[0], right_arm_position_after_rotation[1], right_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ right_arm_orientation_default = optas.spatialmath.Quaternion(1., 0., 0., 0.)
+ right_arm_orientation_quaternion = right_arm_orientation_default.__mul__(Donkey_quaternion)
+ right_arm_quaternion = [right_arm_orientation_quaternion.split()[0], right_arm_orientation_quaternion.split()[1], right_arm_orientation_quaternion.split()[2], right_arm_orientation_quaternion.split()[3]]
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=right_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse left arm arguments
+ left_arm_position_defaut = np.asarray([0.9, 0.12, 0.955])
+ left_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ left_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[left_arm_position_after_rotation[0], left_arm_position_after_rotation[1], left_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ left_arm_quaternion = right_arm_quaternion
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=left_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=5.0)
+ args = vars(parser.parse_args())
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_planner', anonymous=True)
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient(rospy.get_name(),
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ # execute node
+ rospy.spin()
diff --git a/script/action_client_cmd_pose_wholebody.py b/script/action_client_cmd_pose_wholebody.py
new file mode 100755
index 0000000..2ebd51a
--- /dev/null
+++ b/script/action_client_cmd_pose_wholebody.py
@@ -0,0 +1,145 @@
+#! /usr/bin/env python3
+import argparse
+
+import rospy
+import actionlib
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseGoal
+import numpy as np
+import optas
+
+class CmdPoseClient(object):
+ """docstring for CmdPoseClient."""
+
+ def __init__(self, name, target_pos_Donkey, target_quat_Donkey, target_pos_R, target_quat_R, target_pos_L, target_quat_L, duration) -> None:
+ # initialization message
+ self._name = name
+ self._target_pos_Donkey = target_pos_Donkey
+ self._target_quat_Donkey = target_quat_Donkey
+ self._target_pos_R = target_pos_R
+ self._target_quat_R = target_quat_R
+ self._target_pos_L = target_pos_L
+ self._target_quat_L = target_quat_L
+ self._duration = duration
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ # create actionlib client
+ self._action_client = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdChonkPoseAction)
+ # wait until actionlib server starts
+ rospy.loginfo("%s: Waiting for action server to start.", self._name)
+ self._action_client.wait_for_server()
+ rospy.loginfo("%s: Action server started, sending goal.", self._name)
+ # creates goal and sends to the action server
+ goal = CmdChonkPoseGoal()
+ goal.poseDonkey.position.x = self._target_pos_Donkey[0]
+ goal.poseDonkey.position.y = self._target_pos_Donkey[1]
+ goal.poseDonkey.position.z = self._target_pos_Donkey[2]
+ goal.poseDonkey.orientation.x = self._target_quat_Donkey[0]
+ goal.poseDonkey.orientation.y = self._target_quat_Donkey[1]
+ goal.poseDonkey.orientation.z = self._target_quat_Donkey[2]
+ goal.poseDonkey.orientation.w = self._target_quat_Donkey[3]
+ goal.poseR.position.x = self._target_pos_R[0]
+ goal.poseR.position.y = self._target_pos_R[1]
+ goal.poseR.position.z = self._target_pos_R[2]
+ goal.poseR.orientation.x = self._target_quat_R[0]
+ goal.poseR.orientation.y = self._target_quat_R[1]
+ goal.poseR.orientation.z = self._target_quat_R[2]
+ goal.poseR.orientation.w = self._target_quat_R[3]
+ goal.poseL.position.x = self._target_pos_L[0]
+ goal.poseL.position.y = self._target_pos_L[1]
+ goal.poseL.position.z = self._target_pos_L[2]
+ goal.poseL.orientation.x = self._target_quat_L[0]
+ goal.poseL.orientation.y = self._target_quat_L[1]
+ goal.poseL.orientation.z = self._target_quat_L[2]
+ goal.poseL.orientation.w = self._target_quat_L[3]
+ goal.duration = self._duration
+ # sends the goal to the action server
+ rospy.loginfo("%s: Send goal request to action server.", self._name)
+ self._action_client.send_goal(
+ goal,
+ done_cb=self.done_cb,
+ active_cb=self.active_cb,
+ feedback_cb=self.feedback_cb
+ )
+ # wait for the server to finish the action
+ self._action_client.wait_for_result()
+ rospy.loginfo("%s: Got result from action server.", self._name)
+
+ def done_cb(self, state, result):
+ rospy.loginfo("%s: Action completed with result %r" % (self._name, result.reached_goal))
+ rospy.signal_shutdown("Client request completed.")
+
+ def active_cb(self):
+ rospy.loginfo("%s: Goal went active!", self._name)
+
+ def feedback_cb(self, feedback):
+ rospy.loginfo("%s: %.1f%% to completion." % (self._name, feedback.progress))
+
+if __name__ == '__main__':
+ # parse arguments from terminal
+ parser = argparse.ArgumentParser(description='Client node to command robot base and end-effector pose.')
+ # parse donkey arguments
+ Donkey_position = [5., 4., 0.]
+ parser.add_argument('--target_position_Donkey', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=Donkey_position,
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ Donkey_angle = optas.deg2rad(90.)
+ Donkey_quat_w = np.cos(Donkey_angle/2)
+ Donkey_quat_x = 0
+ Donkey_quat_y = 0
+ Donkey_quat_z = np.sin(Donkey_angle/2)
+ Donkey_quaternion = optas.spatialmath.Quaternion(Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_w)
+ parser.add_argument('--target_orientation_Donkey', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=[Donkey_quat_x, Donkey_quat_y, Donkey_quat_z, Donkey_quat_z],
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse right arm arguments
+ right_arm_position_defaut = np.asarray([0.9, -0.12, 0.955])
+ right_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ right_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+
+ parser.add_argument('--target_position_R', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[right_arm_position_after_rotation[0], right_arm_position_after_rotation[1], right_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ right_arm_orientation_default = optas.spatialmath.Quaternion(1., 0., 0., 0.)
+ right_arm_orientation_quaternion = right_arm_orientation_default.__mul__(Donkey_quaternion)
+ right_arm_quaternion = [right_arm_orientation_quaternion.split()[0], right_arm_orientation_quaternion.split()[1], right_arm_orientation_quaternion.split()[2], right_arm_orientation_quaternion.split()[3]]
+ parser.add_argument('--target_orientation_R', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=right_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+ # parse left arm arguments
+ left_arm_position_defaut = np.asarray([0.9, 0.12, 0.955])
+ left_arm_position_after_rotation = optas.spatialmath.rotz(Donkey_angle) @ left_arm_position_defaut.transpose() + np.asarray(Donkey_position)
+ parser.add_argument('--target_position_L', nargs=3,
+ help="Give target position of the robot in meters.",
+ type=float, default=[left_arm_position_after_rotation[0], left_arm_position_after_rotation[1], left_arm_position_after_rotation[2]],
+ metavar=('POS_X', 'POS_Y', 'POS_Z')
+ )
+ left_arm_quaternion = right_arm_quaternion
+ parser.add_argument('--target_orientation_L', nargs=4,
+ help="Give target orientation as a quaternion.",
+ type=float, default=left_arm_quaternion,
+ metavar=('QUAT_X','QUAT_Y','QUAT_Z','QUAT_W')
+ )
+
+ parser.add_argument("--duration", help="Give duration of motion in seconds.", type=float, default=5.0)
+ args = vars(parser.parse_args())
+
+ # Initialize node
+ rospy.init_node('cmd_pose_client_wholebody', anonymous=True)
+ # Initialize node class
+ cmd_pose_client = CmdPoseClient(rospy.get_name(),
+ args['target_position_Donkey'],
+ args['target_orientation_Donkey'],
+ args['target_position_R'],
+ args['target_orientation_R'],
+ args['target_position_L'],
+ args['target_orientation_L'],
+ args['duration']
+ )
+ # execute node
+ rospy.spin()
diff --git a/script/action_client_teleop_2d.py b/script/action_client_teleop_2d.py
new file mode 100755
index 0000000..c8dff57
--- /dev/null
+++ b/script/action_client_teleop_2d.py
@@ -0,0 +1,149 @@
+#!/usr/bin/env python3
+
+import rospy
+import actionlib
+from sensor_msgs.msg import Joy
+from pushing_msgs.msg import TriggerCmdAction, TriggerCmdGoal
+from pushing_msgs.msg import CmdDualPoseAction, CmdDualPoseGoal
+
+class TeleOp2DClient(object):
+ """docstring for TeleOp2DClient."""
+
+ def __init__(self, name) -> None:
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initialized action client class.", self._name)
+ #################################################################################
+ # get parameters
+ self._freq = rospy.get_param('~freq', 100)
+ self._duration = rospy.get_param('~duration', 2.0)
+ self._target_pos_x_R = rospy.get_param('~target_position_x_R', 0.3)
+ self._target_pos_y_R = rospy.get_param('~target_position_y_R', -0.2)
+ self._target_pos_z_R = rospy.get_param('~target_position_z_R', 0.79)
+ self._safety_pos_x_R = rospy.get_param('~safety_position_x_R', 0.3)
+ self._safety_pos_y_R = rospy.get_param('~safety_position_y_R', -0.1)
+ self._safety_pos_z_R = rospy.get_param('~safety_position_z_R', 0.95)
+ self._target_pos_x_L = rospy.get_param('~target_position_x_L', 0.3)
+ self._target_pos_y_L = rospy.get_param('~target_position_y_L', 0.2)
+ self._target_pos_z_L = rospy.get_param('~target_position_z_L', 0.90)
+ self._safety_pos_x_L = rospy.get_param('~safety_position_x_L', 0.3)
+ self._safety_pos_y_L = rospy.get_param('~safety_position_y_L', 0.1)
+ self._safety_pos_z_L = rospy.get_param('~safety_position_z_L', 0.95)
+ # declare joystick substriber
+ self._joy_sub = rospy.Subscriber(
+ "/spacenav/joy",
+ Joy,
+ self.read_joy_status_cb
+ )
+ # declare timer for the State Machine loop
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+ #################################################################################
+ # initialized variables
+ self.joyButtonLeft = False
+ self.joyButtonRight = False
+ self.reachedGoal = False
+ # initialize SM states
+ self.stateInactive = True
+ self.stateTeleOp = False
+ self.stateSend2Target = False
+ self.stateSend2Safety = False
+ # start innactive state
+ self.inactive_cb()
+ #################################################################################
+ # create action client to teleop robot
+ self.action_client_teleop = actionlib.SimpleActionClient('/chonk/teleop_2d', TriggerCmdAction)
+ # creates goal and sends to the action server
+ self.goal_teleop = TriggerCmdGoal(pos_z=self._target_pos_z_R)
+ #################################################################################
+ # create action client for commanding pose
+ self.action_client_cmd_pose = actionlib.SimpleActionClient('/chonk/cmd_pose', CmdDualPoseAction)
+ # create goal
+ self.goal_cmd_pose = CmdDualPoseGoal()
+ self.goal_cmd_pose.poseR.position.x = self._safety_pos_x_R
+ self.goal_cmd_pose.poseR.position.y = self._safety_pos_y_R
+ self.goal_cmd_pose.poseR.position.z = self._safety_pos_z_R
+ self.goal_cmd_pose.poseR.orientation.x = 1
+ self.goal_cmd_pose.poseR.orientation.y = 0
+ self.goal_cmd_pose.poseR.orientation.z = 0
+ self.goal_cmd_pose.poseR.orientation.w = 0
+ self.goal_cmd_pose.poseL.position.x = self._safety_pos_x_L
+ self.goal_cmd_pose.poseL.position.y = self._safety_pos_y_L
+ self.goal_cmd_pose.poseL.position.z = self._safety_pos_z_L
+ self.goal_cmd_pose.poseL.orientation.x = 1
+ self.goal_cmd_pose.poseL.orientation.y = 0
+ self.goal_cmd_pose.poseL.orientation.z = 0
+ self.goal_cmd_pose.poseL.orientation.w = 0
+ self.goal_cmd_pose.duration = self._duration
+ #################################################################################
+
+ def read_joy_status_cb(self, msg):
+ options = [False, True]
+ self.joyButtonLeft = options[msg.buttons[0]]
+ self.joyButtonRight = options[msg.buttons[1]]
+
+ def timer_cb(self, event):
+ """ Manage State Machine logic """
+ if self.stateInactive and self.joyButtonLeft:
+ self.stateInactive = False
+ self.stateSend2Target = True
+ self.send_cb(
+ self._target_pos_x_R, self._target_pos_y_R, self._target_pos_z_R,
+ self._target_pos_x_L, self._target_pos_y_L, self._target_pos_z_L,
+ 'Workspace')
+ elif self.stateSend2Target and self.reachedGoal:
+ self.stateSend2Target = False
+ self.reachedGoal = False
+ self.stateTeleOp = True
+ self.teleop_cb()
+ elif self.stateTeleOp and self.joyButtonRight:
+ self.stateTeleOp = False
+ self.stateSend2Safety = True
+ self.action_client_teleop.cancel_goal()
+ self.send_cb(
+ self._safety_pos_x_R, self._safety_pos_y_R, self._safety_pos_z_R,
+ self._safety_pos_x_L, self._safety_pos_y_L, self._safety_pos_z_L,
+ 'Safety')
+ elif self.stateSend2Safety and self.reachedGoal:
+ self.stateSend2Safety = False
+ self.reachedGoal = False
+ self.stateInactive = True
+ self.inactive_cb()
+ else:
+ pass
+
+ def inactive_cb(self):
+ rospy.loginfo('%s: Inactive State. Press the left bottom to start.' % self._name)
+
+ def teleop_cb(self):
+ rospy.loginfo('%s: Tele-operation State. Press the right bottom to stop.' % self._name)
+ # wait until actionlib server starts
+ self.action_client_teleop.wait_for_server()
+ # sends the goal to the action server
+ self.action_client_teleop.send_goal(self.goal_teleop)
+
+ def send_cb(self, pos_x_R, pos_y_R, pos_z_R, pos_x_L, pos_y_L, pos_z_L, string):
+ rospy.loginfo('%s: Sending robot to %s.' % (self._name, string))
+ # change possition
+ self.goal_cmd_pose.poseR.position.x = pos_x_R
+ self.goal_cmd_pose.poseR.position.y = pos_y_R
+ self.goal_cmd_pose.poseR.position.z = pos_z_R
+ self.goal_cmd_pose.poseL.position.x = pos_x_L
+ self.goal_cmd_pose.poseL.position.y = pos_y_L
+ self.goal_cmd_pose.poseL.position.z = pos_z_L
+ # wait for server
+ self.action_client_cmd_pose.wait_for_server()
+ # send goal
+ self.action_client_cmd_pose.send_goal_and_wait(self.goal_cmd_pose)
+ # get result
+ result = self.action_client_cmd_pose.get_result()
+ self.reachedGoal = result.reached_goal
+
+
+if __name__ == '__main__':
+ # Initialize node
+ rospy.init_node('teleop_2d_client', anonymous=True)
+ # Initialize node class
+ cmd_config_client = TeleOp2DClient(rospy.get_name())
+ # execute node
+ rospy.spin()
diff --git a/script/action_server_cmd_config.py b/script/action_server_cmd_config.py
new file mode 100755
index 0000000..cdd971b
--- /dev/null
+++ b/script/action_server_cmd_config.py
@@ -0,0 +1,226 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+
+import rospy
+import actionlib
+
+from urdf_parser_py.urdf import URDF
+
+from sensor_msgs.msg import JointState
+# ROS messages types for the real robot
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from geometry_msgs.msg import Twist
+# ROS messages for command configuration action
+from pushing_msgs.msg import CmdConfigAction, CmdConfigFeedback, CmdConfigResult
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdConfigActionServer(object):
+ """docstring for CmdConfigActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ # robot name
+ # self._robot_name = rospy.get_param('~robot_name', 'iiwa')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 100)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description'
+ if rospy.has_param(param_robot_description):
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ # get joint names and limits
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed' and jnt.type !='continuous')]
+ self.ndof = len(self.joint_names)
+ self.joint_limit_min = [jnt.limit.lower for jnt in self._urdf.joints if (jnt.type != 'fixed' and jnt.type !='continuous')]
+ self.joint_limit_max = [jnt.limit.upper for jnt in self._urdf.joints if (jnt.type != 'fixed' and jnt.type !='continuous')]
+ # initialize variables
+ self.q_curr = np.zeros(self.ndof)
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/chonk/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+ # declare joint publisher
+# self._joint_pub = rospy.Publisher(
+# self._pub_cmd_topic_name,
+# Float64MultiArray,
+# queue_size=10
+# )
+ self._joint_pub = rospy.Publisher(
+ "/chonk/streaming_controller/command",
+ Float64MultiArray,
+ queue_size=10
+ )
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher(
+ "/chonk/donkey_velocity_controller/cmd_vel",
+ Twist,
+ queue_size=10
+ )
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ self._mux_selection = "None"
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ # initialize action messages
+ self._feedback = CmdConfigFeedback()
+ self._result = CmdConfigResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'cmd_config',
+ CmdConfigAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ qT_array = acceped_goal.positions
+ T = acceped_goal.duration
+ # check size of the target vector
+ goal_ndof = len(qT_array)
+ if goal_ndof != self.ndof:
+ rospy.logwarn("%s: Request aborted. Goal has %d positions instead of %d." % (self._name, goal_ndof, self.ndof))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send robot joints to %s in %.1f seconds." % (self._name, qT_array, T))
+ # helper variables
+ self._steps = int(T * self._freq)
+ self._idx = 0
+ q0 = self.q_curr
+ qT = np.clip(
+ qT_array,
+ a_min=self.joint_limit_min,
+ a_max=self.joint_limit_max
+ ) # make sure that request is within joint limits
+ Dq = qT - q0
+ # interpolate between current and target configuration
+ # polynomial obtained for zero speed (3rd order) and acceleratin (5th order)
+ # at the initial and final time
+ # self._q = lambda t: q0 + (3.*((t/T)**2) - 2.*((t/T)**3))*Dq # 3rd order
+ self._q = lambda t: q0 + (10.*((t/T)**3) - 15.*((t/T)**4) + 6.*((t/T)**5))*Dq # 5th order
+ self._t = np.linspace(0., T, self._steps + 1)
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._timer.shutdown()
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+ # compute next configuration with lambda function
+ q_next = self._q(self._t[self._idx])
+ # update message
+ self._msg.data = q_next
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity) # This is for donkey_velocity_controller
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr = np.asarray(list(msg.position)[:self.ndof])
+
+ def read_mux_selection(self, msg):
+ self._mux_selection = msg.data
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_config_server", anonymous=True)
+ # Initialize node class
+ cmd_config_server = CmdConfigActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_4tasks.py b/script/action_server_cmd_pose_4tasks.py
new file mode 100755
index 0000000..7a3fdcd
--- /dev/null
+++ b/script/action_server_cmd_pose_4tasks.py
@@ -0,0 +1,491 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+#np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+
+from sensor_msgs.msg import JointState
+# ROS messages types for the real robot
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+# ROS messages for command configuration action
+# from pushing_msgs.msg import CmdPoseAction, CmdPoseFeedback, CmdPoseResult
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseFeedback, CmdChonkPoseResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+
+from urdf_parser_py.urdf import URDF
+import tf
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 100)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_chonk'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ ### optas
+ ### ---------------------------------------------------------
+ # set up donkey base motion optimization
+ donkey = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_donkey'
+ )
+ self.donkey_name = donkey.get_name()
+ self.ndof = donkey.ndof
+ # nominal robot configuration
+ self.q_nom = optas.DM.zeros(self.ndof)
+ self.donkey_opt_idx = donkey.optimized_joint_indexes
+ self.donkey_param_idx = donkey.parameter_joint_indexes
+ # set up optimization builder
+ builder_donkey = optas.OptimizationBuilder(T=1, robots=[donkey])
+ # get robot state and parameters variables
+ q_var_donkey = builder_donkey.get_robot_states_and_parameters(self.donkey_name)
+ # get end-effector pose as parameters
+ pos_donkey = builder_donkey.add_parameter('pos_donkey', 3)
+ ori_donkey = builder_donkey.add_parameter('ori_donkey', 4)
+ # set variable boudaries
+ builder_donkey.enforce_model_limits(self.donkey_name)
+ # equality constraint on donkey position
+ donkey_pos_fnc = donkey.get_global_link_position_function(link=self._link_donkey)
+ builder_donkey.add_equality_constraint('final_pos_donkey', donkey_pos_fnc(q_var_donkey), rhs=pos_donkey)
+ # rotation of the donkey
+ self.donkey_fnc = donkey.get_global_link_rotation_function(link=self._link_donkey)
+ # equality constraint on orientation
+ donkey_ori_fnc = donkey.get_global_link_quaternion_function(link=self._link_donkey)
+ builder_donkey.add_equality_constraint('final_ori_donkey', donkey_ori_fnc(q_var_donkey), rhs=ori_donkey)
+ # optimization cost: close to nominal config
+ builder_donkey.add_cost_term('nom_config', optas.sumsqr(q_var_donkey-self.q_nom))
+ # setup solver
+ self.solver_donkey = optas.CasADiSolver(optimization=builder_donkey.build()).setup('ipopt')
+ ### ---------------------------------------------------------
+ # set up right arm optimizataion
+ right_arm = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5'],
+ name='chonk_right_arm'
+ )
+ self.right_arm_name = right_arm.get_name()
+ self.ndof = right_arm.ndof
+ # nominal robot configuration
+ self.q_nom = optas.DM.zeros(self.ndof)
+ self.right_arm_opt_idx = right_arm.optimized_joint_indexes
+ self.right_arm_param_idx = right_arm.parameter_joint_indexes
+ # set up optimization builder
+ builder_right_arm = optas.OptimizationBuilder(T=1, robots=[right_arm])
+ # get robot state and parameters variables
+ q_var = builder_right_arm.get_robot_states_and_parameters(self.right_arm_name)
+ # get end-effector pose as parameters
+ pos = builder_right_arm.add_parameter('pos', 3)
+ ori = builder_right_arm.add_parameter('ori', 4)
+ # set variable boudaries
+ builder_right_arm.enforce_model_limits(self.right_arm_name)
+ # equality constraint on right arm position
+ pos_fnc = right_arm.get_global_link_position_function(link=self._link_ee_right)
+ builder_right_arm.add_equality_constraint('final_pos', pos_fnc(q_var), rhs=pos)
+# builder_right_arm.add_equality_constraint()
+ # rotation of the right arm position
+ self.R_fnc = right_arm.get_global_link_rotation_function(link=self._link_ee_right)
+ # equality constraint on orientation
+ ori_fnc = right_arm.get_global_link_quaternion_function(link=self._link_ee_right)
+ builder_right_arm.add_equality_constraint('final_ori', ori_fnc(q_var), rhs=ori)
+ # optimization cost: close to nominal config
+ builder_right_arm.add_cost_term('nom_config', optas.sumsqr(q_var-self.q_nom))
+ # setup solver
+ self.solver_right_arm = optas.CasADiSolver(optimization=builder_right_arm.build()).setup('ipopt')
+ ### ---------------------------------------------------------
+ # set up head optimization
+ head = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_head'
+ )
+ self.head_name = head.get_name()
+ self.head_opt_idx = head.optimized_joint_indexes
+ self.head_param_idx = head.parameter_joint_indexes
+ # set up optimization builder
+ builder_head = optas.OptimizationBuilder(T=1, robots=[head])
+ # get robot state and parameters variables
+ q_var = builder_head.get_robot_states_and_parameters(self.head_name)
+ # get end-effector pose as parameters
+ pos = builder_head.add_parameter('pos', 3)
+ # get head heading
+ pos_head = head.get_global_link_position_function(link=self._link_head)
+ # get gaze position
+ self.pos_gaze_fnc = head.get_global_link_position_function(link=self._link_gaze)
+ # set variable boudaries
+ builder_head.enforce_model_limits(self.head_name)
+ # optimization cost: close to nominal config
+ builder_head.add_cost_term('heading', optas.norm_2(pos_head(q_var)-pos))
+ # setup solver
+ self.solver_head = optas.CasADiSolver(optimization=builder_head.build()).setup('ipopt')
+ ### ---------------------------------------------------------
+ # set up left arm optimizataion
+ left_arm = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_left_arm'
+ )
+ self.left_arm_name = left_arm.get_name()
+ # nominal robot configuration
+ self.left_arm_opt_idx = left_arm.optimized_joint_indexes
+ self.left_arm_param_idx = left_arm.parameter_joint_indexes
+ # set up optimization builder
+ builder_left_arm = optas.OptimizationBuilder(T=1, robots=[left_arm])
+ # get robot state and parameters variables
+ q_var = builder_left_arm.get_robot_states_and_parameters(self.left_arm_name)
+ q_opt = builder_left_arm.get_model_states(self.left_arm_name)
+ # get end-effector pose as parameters
+ pos = builder_left_arm.add_parameter('pos', 3)
+ # set variable boudaries
+ builder_left_arm.enforce_model_limits(self.left_arm_name)
+ # equality constraint on position
+ pos_fnc = left_arm.get_global_link_position_function(link=self._link_ee_left)
+ builder_left_arm.add_equality_constraint('final_pos', pos_fnc(q_var), rhs=pos)
+ # optimization cost: close to nominal config
+ builder_left_arm.add_cost_term('nom_config', optas.sumsqr(q_opt-self.q_nom[self.left_arm_opt_idx]))
+ # setup solver
+ self.solver_left_arm = optas.CasADiSolver(optimization=builder_left_arm.build()).setup('ipopt')
+ ### ---------------------------------------------------------
+ # initialize variables
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.joint_names = []
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/chonk/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+ self._joint_sub_base = rospy.Subscriber(
+ "/chonk/donkey_velocity_controller/odom",
+ Odometry,
+ self.read_base_states_cb
+ )
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher(
+ self._pub_cmd_topic_name,
+ Float64MultiArray,
+ queue_size=10
+ )
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher(
+ "/chonk/donkey_velocity_controller/cmd_vel",
+ Twist,
+ queue_size=10
+ )
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ # initialize action messages
+ self._feedback = CmdChonkPoseFeedback()
+ self._result = CmdChonkPoseResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'cmd_pose',
+ CmdChonkPoseAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ Donkey_pos = np.asarray([
+ acceped_goal.poseDonkey.position.x,
+ acceped_goal.poseDonkey.position.y,
+ acceped_goal.poseDonkey.position.z,
+ ])
+ Donkey_ori = np.asarray([
+ acceped_goal.poseDonkey.orientation.x,
+ acceped_goal.poseDonkey.orientation.y,
+ acceped_goal.poseDonkey.orientation.z,
+ acceped_goal.poseDonkey.orientation.w
+ ])
+ pos_R = np.asarray([
+ acceped_goal.poseR.position.x,
+ acceped_goal.poseR.position.y,
+ acceped_goal.poseR.position.z
+ ])
+ pos_L = np.asarray([
+ acceped_goal.poseL.position.x,
+ acceped_goal.poseL.position.y,
+ acceped_goal.poseL.position.z
+ ])
+ ori_T = np.asarray([
+ acceped_goal.poseR.orientation.x,
+ acceped_goal.poseR.orientation.y,
+ acceped_goal.poseR.orientation.z,
+ acceped_goal.poseR.orientation.w
+ ])
+ # check boundaries of the position
+ if (pos_R > self._pos_max).any() or (pos_R < self._pos_min).any():
+ rospy.logwarn("%s: Request aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_R[0], pos_R[1], pos_R[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ if (pos_L > self._pos_max).any() or (pos_L < self._pos_min).any():
+ rospy.logwarn("%s: Lequest aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_L[0], pos_L[1], pos_L[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send Doneky to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to (%.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name,
+ Donkey_pos[0], Donkey_pos[1], Donkey_pos[2],
+ Donkey_ori[0], Donkey_ori[1], Donkey_ori[2], Donkey_ori[3],
+ pos_R[0], pos_R[1], pos_R[2],
+ ori_T[0], ori_T[1], ori_T[2], ori_T[3],
+ pos_L[0], pos_L[1], pos_L[2],
+ acceped_goal.duration
+ )
+ )
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ ### optas
+ ### ---------------------------------------------------------
+ ### Donkey problem
+ # set initial seed
+ self.solver_donkey.reset_initial_seed({f'{self.donkey_name}/q': self.q_nom[self.donkey_opt_idx]})
+ self.solver_donkey.reset_parameters({'pos_donkey': Donkey_pos, 'ori_donkey': Donkey_ori, f'{self.donkey_name}/P': self.q_nom[self.donkey_param_idx]})
+ # solve problem
+ solution = self.solver_donkey.solve()
+ qT_array = solution[f'{self.donkey_name}/q']
+ # save solution
+ qT[self.donkey_opt_idx] = np.asarray(qT_array).T[0]
+ qT[self.donkey_param_idx] = np.asarray(self.q_nom[self.donkey_param_idx]).T[0]
+ self.q_nom = qT
+
+ ### ---------------------------------------------------------
+ ### right hand problem
+ # set initial seed
+ self.solver_right_arm.reset_initial_seed({f'{self.right_arm_name}/q': self.q_nom[self.right_arm_opt_idx]})
+ self.solver_right_arm.reset_parameters({'pos': pos_R, 'ori': ori_T, f'{self.right_arm_name}/P': self.q_nom[self.right_arm_param_idx]})
+ # solve problem
+ solution = self.solver_right_arm.solve()
+ qT_array = solution[f'{self.right_arm_name}/q']
+ # save solution
+ qT[self.right_arm_opt_idx] = np.asarray(qT_array).T[0]
+# qT[self.right_arm_param_idx] = np.asarray(self.q_nom[self.right_arm_param_idx]).T[0]
+ self.q_nom = qT
+
+ ### ---------------------------------------------------------
+ ### head problem
+ # set initial seed
+ self.solver_head.reset_initial_seed({f'{self.head_name}/q': qT[self.head_opt_idx]})
+ self.solver_head.reset_parameters({'pos': self.pos_gaze_fnc(qT), f'{self.head_name}/P': qT[self.head_param_idx]})
+ # solve problem
+ solution = self.solver_head.solve()
+ qT_array = solution[f'{self.head_name}/q']
+ # save solution
+ qT[self.head_opt_idx] = np.asarray(qT_array).T[0]
+ self.q_nom = qT
+
+ ### ---------------------------------------------------------
+ ### left arm problem
+ # set initial seed
+ self.solver_left_arm.reset_initial_seed({f'{self.left_arm_name}/q': qT[self.left_arm_opt_idx]})
+ self.solver_left_arm.reset_parameters({'pos': pos_L, f'{self.left_arm_name}/P': qT[self.left_arm_param_idx]})
+ # solve problem
+ solution = self.solver_left_arm.solve()
+ qT_array = solution[f'{self.left_arm_name}/q']
+ # save solution
+ qT[self.left_arm_opt_idx] = np.asarray(qT_array).T[0]
+ ### ---------------------------------------------------------
+ # helper variables
+ q0 = self.q_curr
+ T = acceped_goal.duration
+ # print goal request
+ rospy.loginfo("%s: Request to send robot joints to %s in %.1f seconds." % (self._name, qT, T))
+ self._steps = int(T * self._freq)
+ self._idx = 0
+ Dq = qT - q0
+ # interpolate between current and target configuration
+ # polynomial obtained for zero speed (3rd order) and acceleratin (5th order)
+ # at the initial and final time
+ # self._q = lambda t: q0 + (3.*((t/T)**2) - 2.*((t/T)**3))*Dq # 3rd order
+ self._q = lambda t: q0 + (10.*((t/T)**3) - 15.*((t/T)**4) + 6.*((t/T)**5))*Dq # 5th order
+ self._dq = lambda t: (30.*((t/T)**2) - 60.*((t/T)**3) +30.*((t/T)**4))*(Dq/T)
+ self._t = np.linspace(0., T, self._steps + 1)
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+ # compute next configuration with lambda function
+ q_next = self._q(self._t[self._idx])
+ dq_next = self._dq(self._t[self._idx])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+# - self.donkey_R.T @ optas.spatialmath.skew(self.donkey_position) @ Global_w_b
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names = msg.name
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_4tasks", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC.py b/script/action_server_cmd_pose_MPC.py
new file mode 100755
index 0000000..094ddfe
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC.py
@@ -0,0 +1,486 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseFeedback, CmdChonkPoseResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body motion optimization
+ wholebodyPlanner = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=[],
+ name='chonk_wholebodyPlanner'
+ )
+ self.wholebodyPlanner_name = wholebodyPlanner.get_name()
+ self.ndof = wholebodyPlanner.ndof
+ # nominal robot configuration
+ self.q_nom = optas.DM.zeros(self.ndof)
+ self.wholebodyPlanner_opt_idx = wholebodyPlanner.optimized_joint_indexes
+ self.wholebodyPlanner_param_idx = wholebodyPlanner.parameter_joint_indexes
+ # set up optimization builder
+ builder_wholebodyPlanner = optas.OptimizationBuilder(T=1, robots=[wholebodyPlanner])
+ # get robot state and parameters variables
+ q_var = builder_wholebodyPlanner.get_robot_states_and_parameters(self.wholebodyPlanner_name)
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyPlanner.add_parameter('pos_R', 3)
+ ori_R = builder_wholebodyPlanner.add_parameter('ori_R', 4)
+ pos_L = builder_wholebodyPlanner.add_parameter('pos_L', 3)
+ ori_L = builder_wholebodyPlanner.add_parameter('ori_L', 4)
+ # set variable boudaries
+ builder_wholebodyPlanner.enforce_model_limits(self.wholebodyPlanner_name)
+ # equality constraint on right and left arm positions
+ pos_fnc_Right = wholebodyPlanner.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = wholebodyPlanner.get_global_link_position_function(link=self._link_ee_left)
+ builder_wholebodyPlanner.add_equality_constraint('final_pos_Right', pos_fnc_Right(q_var), rhs=pos_R)
+ builder_wholebodyPlanner.add_equality_constraint('final_pos_Left', pos_fnc_Left(q_var), rhs=pos_L)
+ # rotation of the right and left arms
+ self.Rotation_fnc_Right = wholebodyPlanner.get_global_link_rotation_function(link=self._link_ee_right)
+ self.Rotation_fnc_Left = wholebodyPlanner.get_global_link_rotation_function(link=self._link_ee_left)
+ # equality constraint on orientations
+ ori_fnc_Right = wholebodyPlanner.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = wholebodyPlanner.get_global_link_quaternion_function(link=self._link_ee_left)
+ builder_wholebodyPlanner.add_equality_constraint('final_ori_Right', ori_fnc_Right(q_var), rhs=ori_R)
+ builder_wholebodyPlanner.add_equality_constraint('final_ori_Left', ori_fnc_Left(q_var), rhs=ori_L)
+ # optimization cost: close to nominal config
+ builder_wholebodyPlanner.add_cost_term('nom_config', optas.sumsqr(q_var-self.q_nom))
+ # setup solver
+ self.solver_wholebodyPlanner = optas.CasADiSolver(optimization=builder_wholebodyPlanner.build()).setup('ipopt')
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_name = wholebodyMPC.get_name()
+ self.dt_MPC = 0.02 # time step
+ self.T_MPC = 8 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=self.T_MPC, robots=[wholebodyMPC], derivs_align=True)
+ # get robot state variables, get velocity state variables
+ q_var_MPC = builder_wholebodyMPC.get_robot_states_and_parameters(self.wholebodyMPC_name)
+ dq_var_MPC = builder_wholebodyMPC.get_model_states(self.wholebodyMPC_name, time_deriv=1)
+ p_epsilon = builder_wholebodyMPC.add_decision_variables('p_epsilon', self.ndof)
+# v_epsilon = builder_wholebodyMPC.add_decision_variables('v_epsilon', self.ndof)
+ # set variable boudaries
+ builder_wholebodyMPC.enforce_model_limits(self.wholebodyMPC_name, time_deriv=0)
+ builder_wholebodyMPC.enforce_model_limits(self.wholebodyMPC_name, time_deriv=1)
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ goal = builder_wholebodyMPC.add_parameter('goal', self.ndof, self.T_MPC) # trajectory points
+ dgoal = builder_wholebodyMPC.add_parameter('dgoal', self.ndof, self.T_MPC) # trajectory point velocities
+ # Constraint: dynamics
+ builder_wholebodyMPC.integrate_model_states(self.wholebodyMPC_name, time_deriv=1, dt=self.dt_MPC)
+ builder_wholebodyMPC.add_bound_inequality_constraint('init_p_equal', lhs=init_position_MPC - np.full((self.ndof, 1), 0.01), mid=q_var_MPC[0:self.ndof, 0] + p_epsilon, rhs=init_position_MPC + np.full((self.ndof, 1), 0.01))
+# builder_wholebodyMPC.add_bound_inequality_constraint('init_v_equal', lhs=init_velocity_MPC - np.full((self.ndof, 1), 0.01), mid=dq_var_MPC[0:self.ndof, 0] + v_epsilon, rhs=init_velocity_MPC + np.full((self.ndof, 1), 0.01))
+
+ # Constraint: initial state
+# builder_wholebodyMPC.fix_configuration(self.wholebodyMPC_name, config=init_position_MPC)
+# builder_wholebodyMPC.fix_configuration(self.wholebodyMPC_name, config=init_velocity_MPC, time_deriv=1)
+ # optimization cost: close to nominal config
+ builder_wholebodyMPC.add_cost_term('goal_MPC', optas.sumsqr(q_var_MPC - goal))
+ builder_wholebodyMPC.add_cost_term('dgoal_MPC', optas.sumsqr(dq_var_MPC - dgoal))
+ builder_wholebodyMPC.add_cost_term('minimize_p_epsilon', optas.sumsqr(p_epsilon))
+# builder_wholebodyMPC.add_cost_term('minimize_v_epsilon', optas.sumsqr(v_epsilon))
+ # Cost: minimize velocity
+# w_dq = 0.01/float(self.T_MPC) # weight on cost term
+# builder_wholebodyMPC.add_cost_term('minimize_velocity', w_dq*optas.sumsqr(dq_var_MPC))
+ # Cost: minimize acceleration
+# w_ddq = 0.0025/float(self.T_MPC) # weight on cost term
+# ddq_var_MPC = (dq_var_MPC[:, 1:] - dq_var_MPC[:, :-1])/self.dt_MPC
+# builder_wholebodyMPC.add_cost_term('minimize_acceleration', w_ddq*optas.sumsqr(ddq_var_MPC))
+ # setup solver
+# solver_options={'ipopt.print_level': 0, 'print_time': 0}
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options)
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0})
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('ipopt')
+ self.ti_MPC = 0 # time index of the MPC
+ self.solution_MPC = None
+ ### ---------------------------------------------------------
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/chonk/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+# self._joint_sub_base = rospy.Subscriber(
+# "/chonk/donkey_velocity_controller/odom",
+# Odometry,
+# self.read_base_states_cb
+# )
+ self._joint_sub_base = rospy.Subscriber(
+ "/chonk/base_pose_ground_truth",
+ Odometry,
+ self.read_base_states_cb
+ )
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher(
+ self._pub_cmd_topic_name,
+ Float64MultiArray,
+ queue_size=10
+ )
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher(
+ "/chonk/donkey_velocity_controller/cmd_vel",
+ Twist,
+ queue_size=10
+ )
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ # initialize action messages
+ self._feedback = CmdChonkPoseFeedback()
+ self._result = CmdChonkPoseResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'cmd_pose',
+ CmdChonkPoseAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([
+ acceped_goal.poseR.position.x,
+ acceped_goal.poseR.position.y,
+ acceped_goal.poseR.position.z
+ ])
+ pos_Left = np.asarray([
+ acceped_goal.poseL.position.x,
+ acceped_goal.poseL.position.y,
+ acceped_goal.poseL.position.z
+ ])
+ ori_Right = np.asarray([
+ acceped_goal.poseR.orientation.x,
+ acceped_goal.poseR.orientation.y,
+ acceped_goal.poseR.orientation.z,
+ acceped_goal.poseR.orientation.w
+ ])
+ ori_Left = np.asarray([
+ acceped_goal.poseL.orientation.x,
+ acceped_goal.poseL.orientation.y,
+ acceped_goal.poseL.orientation.z,
+ acceped_goal.poseL.orientation.w
+ ])
+ # check boundaries of the position
+ if (pos_Right > self._pos_max).any() or (pos_Right < self._pos_min).any():
+ rospy.logwarn("%s: Request aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Right[0], pos_Right[1], pos_Right[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ if (pos_Left > self._pos_max).any() or (pos_Left < self._pos_min).any():
+ rospy.logwarn("%s: Lequest aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Left[0], pos_Left[1], pos_Left[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name,
+ pos_Right[0], pos_Right[1], pos_Right[2],
+ ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2],
+ ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3],
+ acceped_goal.duration
+ )
+ )
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve whole-body problem
+ # set initial seed
+ self.solver_wholebodyPlanner.reset_initial_seed({f'{self.wholebodyPlanner_name}/q': self.q_nom[self.wholebodyPlanner_opt_idx]})
+ self.solver_wholebodyPlanner.reset_parameters({'pos_R': pos_Right, 'ori_R': ori_Right, 'pos_L': pos_Left, 'ori_L': ori_Left, f'{self.wholebodyPlanner_name}/P': self.q_nom[self.wholebodyPlanner_param_idx]})
+# self.solver_wholebody.reset_parameters({'pos_R': pos_Right, 'ori_R': ori_Right, 'pos_L': pos_Left, f'{self.wholebody_name}/P': self.q_nom[self.wholebody_param_idx]})
+ # solve problem
+ solution = self.solver_wholebodyPlanner.solve()
+ qT_array = solution[f'{self.wholebodyPlanner_name}/q']
+ # save solution
+ qT[self.wholebodyPlanner_opt_idx] = np.asarray(qT_array).T[0]
+ qT[self.wholebodyPlanner_param_idx] = np.asarray(self.q_nom[self.wholebodyPlanner_param_idx]).T[0]
+ ### ---------------------------------------------------------
+ # helper variables
+ q0 = self.q_curr
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ Dq = qT - q0
+ # interpolate between current and target configuration polynomial obtained for zero speed (3rd order) and acceleratin (5th order)
+ # at the initial and final time
+ # self._q = lambda t: q0 + (3.*((t/self.duration)**2) - 2.*((t/self.duration)**3))*Dq # 3rd order
+ self._q = lambda t: q0 + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Dq # 5th order
+ self._dq = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Dq/self.duration)
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # print goal request
+ rospy.loginfo("%s: Request to send robot joints to %s in %.1f seconds." % (self._name, qT, self.duration))
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+ # read current robot joint positions
+ goal = []
+ dgoal = []
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ try:
+ g = self._q(self.ti_MPC).flatten()
+ goal.append(g.tolist())
+ except ValueError:
+ goal.append(g.tolist()) # i.e. previous goal
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ try:
+ dg = self._dq(self.ti_MPC).flatten()
+ dgoal.append(dg.tolist())
+ except ValueError:
+ dgoal.append(dg.tolist())
+
+ goal = optas.np.array(goal).T
+ dgoal = optas.np.array(dgoal).T
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+# self.solver_wholebodyMPC.reset_initial_seed({f'{self.wholebodyMPC_name}/q': np.zeros((self.ndof, self.T_MPC)), f'{self.wholebodyMPC_name}/dq': np.zeros((self.ndof, self.T_MPC)), f'p_epsilon': np.zeros(self.ndof), f'v_epsilon': np.zeros(self.ndof)} )
+ self.solver_wholebodyMPC.reset_initial_seed({f'{self.wholebodyMPC_name}/q': np.zeros((self.ndof, self.T_MPC)), f'{self.wholebodyMPC_name}/dq': np.zeros((self.ndof, self.T_MPC)), f'p_epsilon': np.zeros(self.ndof)} )
+# self.solver_wholebodyMPC.reset_initial_seed({f'{self.wholebodyMPC_name}/q': np.zeros((self.ndof, self.T_MPC)), f'{self.wholebodyMPC_name}/dq': np.zeros((self.ndof, self.T_MPC))} )
+
+ # set initial seed
+ if self.solution_MPC is not None:
+# self.solver_wholebodyMPC.reset_initial_seed({f'{self.wholebodyMPC_name}/q': self.solution_MPC[f'{self.wholebodyMPC_name}/q'], f'{self.wholebodyMPC_name}/dq': self.solution_MPC[f'{self.wholebodyMPC_name}/dq'], f'p_epsilon': self.solution_MPC[f'p_epsilon'], f'v_epsilon': self.solution_MPC[f'v_epsilon']})
+ self.solver_wholebodyMPC.reset_initial_seed({f'{self.wholebodyMPC_name}/q': self.solution_MPC[f'{self.wholebodyMPC_name}/q'], f'{self.wholebodyMPC_name}/dq': self.solution_MPC[f'{self.wholebodyMPC_name}/dq'], f'p_epsilon': self.solution_MPC[f'p_epsilon']})
+# self.solver_wholebodyMPC.reset_initial_seed({f'{self.wholebodyMPC_name}/q': self.solution_MPC[f'{self.wholebodyMPC_name}/q'], f'{self.wholebodyMPC_name}/dq': self.solution_MPC[f'{self.wholebodyMPC_name}/dq']})
+# self.solver_wholebodyMPC.reset_parameters({'goal': goal, 'dgoal': dgoal, 'init_position_MPC': self.q_curr})
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ self.solver_wholebodyMPC.reset_parameters({'goal': goal, 'dgoal': dgoal, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr})
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.solve()
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ q_plan = self.solver_wholebodyMPC.interpolate(self.solution_MPC[f'{self.wholebodyMPC_name}/q'], self.duration_MPC)
+ q_next = q_plan(1/self._freq)
+ dq_plan = self.solver_wholebodyMPC.interpolate(self.solution_MPC[f'{self.wholebodyMPC_name}/dq'], self.duration_MPC)
+ dq_next = dq_plan(1/self._freq)
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ base_linear_velocity_global = self.donkey_R @ self.donkey_velocity
+ base_angular_velocity_global = self.donkey_R @ self.donkey_angular_velocity
+# self.dq_curr_base = [base_linear_velocity_global[0], base_linear_velocity_global[1], base_angular_velocity_global[2]]
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC.py b/script/action_server_cmd_pose_MPC_BC.py
new file mode 100755
index 0000000..1a12379
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC.py
@@ -0,0 +1,546 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+#np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseFeedback, CmdChonkPoseResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body motion optimization
+ wholebodyPlanner = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=[],
+ name='chonk_wholebodyPlanner'
+ )
+ self.wholebodyPlanner_name = wholebodyPlanner.get_name()
+ self.ndof = wholebodyPlanner.ndof
+ # nominal robot configuration
+ self.q_nom = optas.DM.zeros(self.ndof)
+ self.wholebodyPlanner_opt_idx = wholebodyPlanner.optimized_joint_indexes
+ self.wholebodyPlanner_param_idx = wholebodyPlanner.parameter_joint_indexes
+ # set up optimization builder
+ builder_wholebodyPlanner = optas.OptimizationBuilder(T=1, robots=[wholebodyPlanner])
+ # get robot state and parameters variables
+ q_var = builder_wholebodyPlanner.get_robot_states_and_parameters(self.wholebodyPlanner_name)
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyPlanner.add_parameter('pos_R', 3)
+ ori_R = builder_wholebodyPlanner.add_parameter('ori_R', 4)
+ pos_L = builder_wholebodyPlanner.add_parameter('pos_L', 3)
+ ori_L = builder_wholebodyPlanner.add_parameter('ori_L', 4)
+ # set variable boudaries
+ builder_wholebodyPlanner.enforce_model_limits(self.wholebodyPlanner_name)
+ # equality constraint on right and left arm positions
+ pos_fnc_Right = wholebodyPlanner.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = wholebodyPlanner.get_global_link_position_function(link=self._link_ee_left)
+ builder_wholebodyPlanner.add_equality_constraint('final_pos_Right', pos_fnc_Right(q_var), rhs=pos_R)
+ builder_wholebodyPlanner.add_equality_constraint('final_pos_Left', pos_fnc_Left(q_var), rhs=pos_L)
+ # rotation of the right and left arms
+ self.Rotation_fnc_Right = wholebodyPlanner.get_global_link_rotation_function(link=self._link_ee_right)
+ self.Rotation_fnc_Left = wholebodyPlanner.get_global_link_rotation_function(link=self._link_ee_left)
+ # equality constraint on orientations
+ ori_fnc_Right = wholebodyPlanner.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = wholebodyPlanner.get_global_link_quaternion_function(link=self._link_ee_left)
+ builder_wholebodyPlanner.add_equality_constraint('final_ori_Right', ori_fnc_Right(q_var), rhs=ori_R)
+ builder_wholebodyPlanner.add_equality_constraint('final_ori_Left', ori_fnc_Left(q_var), rhs=ori_L)
+ # optimization cost: close to nominal config
+ builder_wholebodyPlanner.add_cost_term('nom_config', optas.sumsqr(q_var-self.q_nom))
+ # setup solver
+ self.solver_wholebodyPlanner = optas.CasADiSolver(optimization=builder_wholebodyPlanner.build()).setup('knitro')
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyMPC_LIMITS'
+ )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ # set up the our robot without optimizing the joint configuration directly.
+ wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_name = wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 12 # T_MPC is number of time steps of MPC
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[wholebodyMPC])
+# print(builder_wholebodyMPC._decision_variables)
+# print(builder_wholebodyMPC._parameters)
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+# print(builder_wholebodyMPC._decision_variables)
+# print(builder_wholebodyMPC._parameters)
+ # get robot state variables, get velocity state variables
+ P = builder_wholebodyMPC.add_decision_variables('P', self.ndof * self.T_MPC)
+ p_epsilon = builder_wholebodyMPC.add_decision_variables('p_epsilon', self.ndof)
+# v_epsilon = builder_wholebodyMPC.add_decision_variables('v_epsilon', self.ndof)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ n = self.T_MPC -1 # N in Bezier curve
+ q_var_MPC = optas.casadi.SX(np.zeros(self.ndof * self.T_MPC))
+
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ goal = builder_wholebodyMPC.add_parameter('goal', self.ndof * self.T_MPC) # trajectory points
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[i*self.ndof: (i+1)*self.ndof] += self.BC(n, j) * t[i]**j * (1-t[i])**(n-j) * P[j*self.ndof: (j+1)*self.ndof]
+ name = 'control_point_' + str(i) + '_bound' # add position constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=lower, mid=P[i*self.ndof: (i+1)*self.ndof], rhs=upper)
+ # add position constraint at the beginning state
+# builder_wholebodyMPC.add_equality_constraint('init_equal', lhs=P[0:3], rhs=init_position_MPC[0:3])
+ builder_wholebodyMPC.add_bound_inequality_constraint('init_p_equal', lhs=init_position_MPC - np.full((self.ndof, 1), 0.001), mid=P[0:self.ndof] + p_epsilon, rhs=init_position_MPC + np.full((self.ndof, 1), 0.001))
+
+ # Add parameters
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ dgoal = builder_wholebodyMPC.add_parameter('dgoal', self.ndof * self.T_MPC) # trajectory point velocities
+ dq_var_MPC = optas.casadi.SX(np.zeros(self.ndof * self.T_MPC))
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[i*self.ndof: (i+1)*self.ndof] += self.BC(n-1, j) * t[i]**j * (1-t[i])**(n-1-j) * n * (P[(j+1)*self.ndof: (j+2)*self.ndof] - P[j*self.ndof: (j+1)*self.ndof])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=n * (P[(i+1)*self.ndof: (i+2)*self.ndof] - P[i*self.ndof: (i+1)*self.ndof]), rhs=dupper)
+# builder_wholebodyMPC.add_bound_inequality_constraint('init_v_equal', lhs=init_velocity_MPC - np.full((self.ndof, 1), 0.01), mid=n*(P[self.ndof:2*self.ndof] - P[0:self.ndof])+ v_epsilon, rhs=init_velocity_MPC + np.full((self.ndof, 1), 0.01))
+
+ # add velocity constraint at the beginning state
+# builder_wholebodyMPC.add_equality_constraint(name, lhs=P[self.ndof: 2*self.ndof][0:2], rhs=((init_velocity_MPC/n)+init_position_MPC)[0:2])
+ # optimization cost: close to goal position and velocity
+# w_q = np.diag(np.full(self.T_MPC * self.ndof,1))
+# for i in range(self.T_MPC):
+# w_q[i*self.ndof, i*self.ndof] = 10
+# w_q[i*self.ndof+1, i*self.ndof+1] = 10
+# w_q[i*self.ndof+2, i*self.ndof+2] = 10
+# builder_wholebodyMPC.add_cost_term('goal_MPC', optas.sumsqr(w_q@(q_var_MPC - goal)))
+# builder_wholebodyMPC.add_cost_term('dgoal_MPC', optas.sumsqr((dq_var_MPC - dgoal)))
+ builder_wholebodyMPC.add_cost_term('goal_MPC', optas.sumsqr(q_var_MPC - goal))
+ builder_wholebodyMPC.add_cost_term('dgoal_MPC', optas.sumsqr(dq_var_MPC - dgoal))
+ builder_wholebodyMPC.add_cost_term('minimize_p_epsilon', optas.sumsqr(p_epsilon))
+# builder_wholebodyMPC.add_cost_term('minimize_v_epsilon', optas.sumsqr(v_epsilon))
+
+# builder_wholebodyMPC.add_cost_term('goal_MPC', (q_var_MPC - goal).T @ (q_var_MPC - goal))
+# builder_wholebodyMPC.add_cost_term('dgoal_MPC', 0.02*(dq_var_MPC - dgoal).T @ (dq_var_MPC - dgoal))
+# builder_wholebodyMPC.add_cost_term('dgoal', 0.005*optas.sumsqr(dq_var_MPC))
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('ipopt', solver_options={'ipopt.print_level': 0, 'print_time': 0})
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('ipopt', solver_options={'ipopt.tol': 1E-5})
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('ipopt')
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0} )
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.feastol':1e-3, 'knitro.opttol':1e-3, 'knitro.ftol':1e-2})
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ self.ti_MPC = 0 # time index of the MPC
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+# print(self.timebyT)
+# print(builder_wholebodyMPC._decision_variables)
+# print(builder_wholebodyMPC._parameters)
+ ### ---------------------------------------------------------
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/chonk/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+# self._joint_sub_base = rospy.Subscriber(
+# "/chonk/donkey_velocity_controller/odom",
+# Odometry,
+# self.read_base_states_cb
+# )
+ self._joint_sub_base = rospy.Subscriber(
+ "/chonk/base_pose_ground_truth",
+ Odometry,
+ self.read_base_states_cb
+ )
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher(
+ self._pub_cmd_topic_name,
+ Float64MultiArray,
+ queue_size=10
+ )
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher(
+ "/chonk/donkey_velocity_controller/cmd_vel",
+ Twist,
+ queue_size=10
+ )
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ # initialize action messages
+ self._feedback = CmdChonkPoseFeedback()
+ self._result = CmdChonkPoseResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'cmd_pose',
+ CmdChonkPoseAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([
+ acceped_goal.poseR.position.x,
+ acceped_goal.poseR.position.y,
+ acceped_goal.poseR.position.z
+ ])
+ pos_Left = np.asarray([
+ acceped_goal.poseL.position.x,
+ acceped_goal.poseL.position.y,
+ acceped_goal.poseL.position.z
+ ])
+ ori_Right = np.asarray([
+ acceped_goal.poseR.orientation.x,
+ acceped_goal.poseR.orientation.y,
+ acceped_goal.poseR.orientation.z,
+ acceped_goal.poseR.orientation.w
+ ])
+ ori_Left = np.asarray([
+ acceped_goal.poseL.orientation.x,
+ acceped_goal.poseL.orientation.y,
+ acceped_goal.poseL.orientation.z,
+ acceped_goal.poseL.orientation.w
+ ])
+ # check boundaries of the position
+ if (pos_Right > self._pos_max).any() or (pos_Right < self._pos_min).any():
+ rospy.logwarn("%s: Request aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Right[0], pos_Right[1], pos_Right[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ if (pos_Left > self._pos_max).any() or (pos_Left < self._pos_min).any():
+ rospy.logwarn("%s: Lequest aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Left[0], pos_Left[1], pos_Left[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name,
+ pos_Right[0], pos_Right[1], pos_Right[2],
+ ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2],
+ ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3],
+ acceped_goal.duration
+ )
+ )
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve whole-body problem
+ # set initial seed
+ self.solver_wholebodyPlanner.reset_initial_seed({f'{self.wholebodyPlanner_name}/q': self.q_nom[self.wholebodyPlanner_opt_idx]})
+ self.solver_wholebodyPlanner.reset_parameters({'pos_R': pos_Right, 'ori_R': ori_Right, 'pos_L': pos_Left, 'ori_L': ori_Left, f'{self.wholebodyPlanner_name}/P': self.q_nom[self.wholebodyPlanner_param_idx]})
+# self.solver_wholebody.reset_parameters({'pos_R': pos_Right, 'ori_R': ori_Right, 'pos_L': pos_Left, f'{self.wholebody_name}/P': self.q_nom[self.wholebody_param_idx]})
+ # solve problem
+ solution = self.solver_wholebodyPlanner.solve()
+ qT_array = solution[f'{self.wholebodyPlanner_name}/q']
+ # save solution
+ qT[self.wholebodyPlanner_opt_idx] = np.asarray(qT_array).T[0]
+ qT[self.wholebodyPlanner_param_idx] = np.asarray(self.q_nom[self.wholebodyPlanner_param_idx]).T[0]
+ ### ---------------------------------------------------------
+ # helper variables
+ q0 = self.q_curr
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ Dq = qT - q0
+ # interpolate between current and target configuration polynomial obtained for zero speed (3rd order) and acceleratin (5th order)
+ # at the initial and final time
+ # self._q = lambda t: q0 + (3.*((t/self.duration)**2) - 2.*((t/self.duration)**3))*Dq # 3rd order
+ self._q = lambda t: q0 + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Dq # 5th order
+ self._dq = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Dq/self.duration)
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # print goal request
+ rospy.loginfo("%s: Request to send robot joints to %s in %.1f seconds." % (self._name, qT, self.duration))
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+ goal = []
+ dgoal = []
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ try:
+ g = self._q(self.ti_MPC).flatten()
+ goal.append(g.tolist())
+ except ValueError:
+ goal.append(g.tolist()) # i.e. previous goal
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ try:
+ dg = self._dq(self.ti_MPC).flatten()
+ dgoal.append(dg.tolist())
+ except ValueError:
+ dgoal.append(dg.tolist())
+
+ goal = optas.np.array(goal).flatten()
+ dgoal = optas.np.array(dgoal).flatten()
+
+
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'P': np.zeros(self.ndof * self.T_MPC), f'p_epsilon': np.zeros(self.ndof)} )
+# self.solver_wholebodyMPC.reset_initial_seed({f'P': np.zeros(self.ndof * self.T_MPC), f'p_epsilon': np.zeros(self.ndof), f'v_epsilon': np.zeros(self.ndof)} )
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'P': self.solution_MPC[f'P'], f'p_epsilon': self.solution_MPC[f'p_epsilon']})
+# self.solver_wholebodyMPC.reset_initial_seed({f'P': self.solution_MPC[f'P'], f'p_epsilon': self.solution_MPC[f'p_epsilon'], f'v_epsilon': self.solution_MPC[f'v_epsilon']})
+ self.solver_wholebodyMPC.reset_parameters({'t': self.timebyT, 'init_position_MPC': self.q_curr, 'goal': goal, 'init_velocity_MPC': self.dq_curr, 'dgoal': dgoal } )
+ # solve problem
+# self.solution_MPC = self.solver_wholebodyMPC.solve()
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+# print(self.solution_MPC)
+ P = np.asarray(self.solution_MPC[f'P'])[:,0]
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC
+ n = self.T_MPC -1
+ q_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * P[j*self.ndof: (j+1)*self.ndof]
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P[(j+1)*self.ndof: (j+2)*self.ndof] - P[j*self.ndof: (j+1)*self.ndof])
+# q_next = P[(0)*self.ndof: (1)*self.ndof]
+# dq_next = n * (P[(0+1)*self.ndof: (0+2)*self.ndof] - P[0*self.ndof: (0+1)*self.ndof])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+# if(self.q_curr_base[2]>1 or self.q_curr_base[2]<-1):
+# print(self.q_curr_base[2])
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+# base_linear_velocity_global = self.donkey_R @ self.donkey_velocity
+# base_angular_velocity_global = self.donkey_R @ self.donkey_angular_velocity
+# self.dq_curr_base = [float(base_linear_velocity_global[0]), float(base_linear_velocity_global[1]), float(self.donkey_angular_velocity[2])]
+# print(self.dq_curr_base)
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+# print(base_linear_velocity_global)
+# print(base_angular_velocity_global)
+# print(self.dq_curr_base)
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC2.py b/script/action_server_cmd_pose_MPC_BC2.py
new file mode 100755
index 0000000..9ffb926
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC2.py
@@ -0,0 +1,476 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseFeedback, CmdChonkPoseResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body motion optimization
+ wholebodyPlanner = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=[],
+ name='chonk_wholebodyPlanner'
+ )
+ self.wholebodyPlanner_name = wholebodyPlanner.get_name()
+ self.ndof = wholebodyPlanner.ndof
+ # nominal robot configuration
+ self.q_nom = optas.DM.zeros(self.ndof)
+ self.wholebodyPlanner_opt_idx = wholebodyPlanner.optimized_joint_indexes
+ self.wholebodyPlanner_param_idx = wholebodyPlanner.parameter_joint_indexes
+ # set up optimization builder
+ builder_wholebodyPlanner = optas.OptimizationBuilder(T=1, robots=[wholebodyPlanner])
+ # get robot state and parameters variables
+ q_var = builder_wholebodyPlanner.get_robot_states_and_parameters(self.wholebodyPlanner_name)
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyPlanner.add_parameter('pos_R', 3)
+ ori_R = builder_wholebodyPlanner.add_parameter('ori_R', 4)
+ pos_L = builder_wholebodyPlanner.add_parameter('pos_L', 3)
+ ori_L = builder_wholebodyPlanner.add_parameter('ori_L', 4)
+ # set variable boudaries
+ builder_wholebodyPlanner.enforce_model_limits(self.wholebodyPlanner_name)
+ # equality constraint on right and left arm positions
+ pos_fnc_Right = wholebodyPlanner.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = wholebodyPlanner.get_global_link_position_function(link=self._link_ee_left)
+ builder_wholebodyPlanner.add_equality_constraint('final_pos_Right', pos_fnc_Right(q_var), rhs=pos_R)
+ builder_wholebodyPlanner.add_equality_constraint('final_pos_Left', pos_fnc_Left(q_var), rhs=pos_L)
+ # rotation of the right and left arms
+ self.Rotation_fnc_Right = wholebodyPlanner.get_global_link_rotation_function(link=self._link_ee_right)
+ self.Rotation_fnc_Left = wholebodyPlanner.get_global_link_rotation_function(link=self._link_ee_left)
+ # equality constraint on orientations
+ ori_fnc_Right = wholebodyPlanner.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = wholebodyPlanner.get_global_link_quaternion_function(link=self._link_ee_left)
+ builder_wholebodyPlanner.add_equality_constraint('final_ori_Right', ori_fnc_Right(q_var), rhs=ori_R)
+ builder_wholebodyPlanner.add_equality_constraint('final_ori_Left', ori_fnc_Left(q_var), rhs=ori_L)
+ # optimization cost: close to nominal config
+ builder_wholebodyPlanner.add_cost_term('nom_config', optas.sumsqr(q_var-self.q_nom))
+ # setup solver
+ self.solver_wholebodyPlanner = optas.CasADiSolver(optimization=builder_wholebodyPlanner.build()).setup('ipopt')
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyMPC_LIMITS'
+ )
+ wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 10 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ P = builder_wholebodyMPC.add_decision_variables('P', self.ndof, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ goal = builder_wholebodyMPC.add_parameter('goal', self.ndof, self.T_MPC) # trajectory points
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P[:, j]
+ name = 'control_point_' + str(i) + '_bound' # add position constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=lower, mid=P[:, i], rhs=upper)
+ # add position constraint at the beginning state
+# builder_wholebodyMPC.add_equality_constraint(name, lhs=q_var_MPC[:, 0], rhs=init_position_MPC)
+ # Add parameters
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ dgoal = builder_wholebodyMPC.add_parameter('dgoal', self.ndof, self.T_MPC) # trajectory point velocities
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ for i in range(self.T_MPC-1):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P[:, j+1] - P[:, j])
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=self.n * (P[:, i+1] - P[:, i]), rhs=dupper)
+ # add velocity constraint at the beginning state
+# builder_wholebodyMPC.add_equality_constraint(name, lhs=dq_var_MPC[:, 0], rhs=init_velocity_MPC)
+ # optimization cost: close to nominal config
+ builder_wholebodyMPC.add_cost_term('goal_MPC', optas.sumsqr(q_var_MPC - goal))
+ builder_wholebodyMPC.add_cost_term('dgoal_MPC', optas.sumsqr(dq_var_MPC - dgoal))
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('ipopt', solver_options={'ipopt.print_level': 0, 'print_time': 0})
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('ipopt')
+ self.ti_MPC = 0 # time index of the MPC
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+ ### ---------------------------------------------------------
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/chonk/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+ self._joint_sub_base = rospy.Subscriber(
+ "/chonk/donkey_velocity_controller/odom",
+ Odometry,
+ self.read_base_states_cb
+ )
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher(
+ self._pub_cmd_topic_name,
+ Float64MultiArray,
+ queue_size=10
+ )
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher(
+ "/chonk/donkey_velocity_controller/cmd_vel",
+ Twist,
+ queue_size=10
+ )
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ # initialize action messages
+ self._feedback = CmdChonkPoseFeedback()
+ self._result = CmdChonkPoseResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'cmd_pose',
+ CmdChonkPoseAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([
+ acceped_goal.poseR.position.x,
+ acceped_goal.poseR.position.y,
+ acceped_goal.poseR.position.z
+ ])
+ pos_Left = np.asarray([
+ acceped_goal.poseL.position.x,
+ acceped_goal.poseL.position.y,
+ acceped_goal.poseL.position.z
+ ])
+ ori_Right = np.asarray([
+ acceped_goal.poseR.orientation.x,
+ acceped_goal.poseR.orientation.y,
+ acceped_goal.poseR.orientation.z,
+ acceped_goal.poseR.orientation.w
+ ])
+ ori_Left = np.asarray([
+ acceped_goal.poseL.orientation.x,
+ acceped_goal.poseL.orientation.y,
+ acceped_goal.poseL.orientation.z,
+ acceped_goal.poseL.orientation.w
+ ])
+ # check boundaries of the position
+ if (pos_Right > self._pos_max).any() or (pos_Right < self._pos_min).any():
+ rospy.logwarn("%s: Request aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Right[0], pos_Right[1], pos_Right[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ if (pos_Left > self._pos_max).any() or (pos_Left < self._pos_min).any():
+ rospy.logwarn("%s: Lequest aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Left[0], pos_Left[1], pos_Left[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name,
+ pos_Right[0], pos_Right[1], pos_Right[2],
+ ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2],
+ ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3],
+ acceped_goal.duration
+ )
+ )
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve whole-body problem
+ # set initial seed
+ self.solver_wholebodyPlanner.reset_initial_seed({f'{self.wholebodyPlanner_name}/q': self.q_nom[self.wholebodyPlanner_opt_idx]})
+ self.solver_wholebodyPlanner.reset_parameters({'pos_R': pos_Right, 'ori_R': ori_Right, 'pos_L': pos_Left, 'ori_L': ori_Left, f'{self.wholebodyPlanner_name}/P': self.q_nom[self.wholebodyPlanner_param_idx]})
+# self.solver_wholebody.reset_parameters({'pos_R': pos_Right, 'ori_R': ori_Right, 'pos_L': pos_Left, f'{self.wholebody_name}/P': self.q_nom[self.wholebody_param_idx]})
+ # solve problem
+ solution = self.solver_wholebodyPlanner.solve()
+ qT_array = solution[f'{self.wholebodyPlanner_name}/q']
+ # save solution
+ qT[self.wholebodyPlanner_opt_idx] = np.asarray(qT_array).T[0]
+ qT[self.wholebodyPlanner_param_idx] = np.asarray(self.q_nom[self.wholebodyPlanner_param_idx]).T[0]
+ ### ---------------------------------------------------------
+ # helper variables
+ q0 = self.q_curr
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ Dq = qT - q0
+ # interpolate between current and target configuration polynomial obtained for zero speed (3rd order) and acceleratin (5th order)
+ # at the initial and final time
+ # self._q = lambda t: q0 + (3.*((t/self.duration)**2) - 2.*((t/self.duration)**3))*Dq # 3rd order
+ self._q = lambda t: q0 + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Dq # 5th order
+ self._dq = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Dq/self.duration)
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # print goal request
+ rospy.loginfo("%s: Request to send robot joints to %s in %.1f seconds." % (self._name, qT, self.duration))
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ goal = []
+ dgoal = []
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx] + self.dt_MPC*i
+ try:
+ g = self._q(self.ti_MPC).flatten()
+ goal.append(g.tolist())
+ dg = self._dq(self.ti_MPC).flatten()
+ dgoal.append(dg.tolist())
+ except ValueError:
+ goal.append(g.tolist()) # i.e. previous goal
+ dgoal.append(dg.tolist())
+
+ goal = optas.np.array(goal).T
+ dgoal = optas.np.array(dgoal).T
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'P': self.solution_MPC[f'P']})
+ self.solver_wholebodyMPC.reset_parameters({'t': self.timebyT, 'init_position_MPC': self.q_curr, 'goal': goal, 'init_velocity_MPC': self.dq_curr, 'dgoal': dgoal } )
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ P = np.asarray(self.solution_MPC[f'P'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ q_next = P[:, 0]
+ dq_next = self.n * (P[:, 1] - P[:, 0])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ base_linear_velocity_global = self.donkey_R @ self.donkey_velocity
+ base_angular_velocity_global = self.donkey_R @ self.donkey_angular_velocity
+ self.dq_curr_base = [base_linear_velocity_global[0], base_linear_velocity_global[1], base_angular_velocity_global[2]]
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC2", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational.py b/script/action_server_cmd_pose_MPC_BC_operational.py
new file mode 100755
index 0000000..3e8c168
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational.py
@@ -0,0 +1,674 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseFeedback, CmdChonkPoseResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+# self._link_sensor_right = rospy.get_param('~link_sensor_right', 'link_sensor_right')
+# self._link_sensor_left = rospy.get_param('~link_sensor_left', 'link_sensor_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 30)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyMPC_LIMITS'
+ )
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 6 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ P = builder_wholebodyMPC.add_decision_variables('P', self.ndof, self.T_MPC)
+# p_epsilon = builder_wholebodyMPC.add_decision_variables('p_epsilon', self.ndof)
+
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+ ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+ pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+ ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+ # functions of right and left arm positions
+ pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+
+ pos_ee_INsensorFrame_Right = np.asarray([-0.08, 0, 0.039+0.04])
+ pos_ee_INsensorFrame_Left = np.asarray([-0.08, 0, 0.039+0.04])
+ rot_ee_INsensorFrame_Right = optas.spatialmath.roty(-np.pi/2)
+ rot_ee_INsensorFrame_Left = optas.spatialmath.roty(-np.pi/2)
+
+ self.X_ee_INsensorFrame_Right = X_fromRandP(rot_ee_INsensorFrame_Right, pos_ee_INsensorFrame_Right)
+ self.X_ee_INsensorFrame_Left = X_fromRandP(rot_ee_INsensorFrame_Left, pos_ee_INsensorFrame_Left)
+
+# self.rot_ee_right_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+# self.rot_ee_left_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+# self.pos_mass_INee_Right = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+# self.pos_mass_INee_Left = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+
+# self.mass_ee_Force = np.asarray([0,0,0,0,0,-0.3113*9.8])
+
+
+
+
+
+ # quaternion functions by given q_var_MPC
+# ori_function_R = optas.casadi.SX(np.zeros((4, self.T_MPC)))
+# ori_function_L = optas.casadi.SX(np.zeros((4, self.T_MPC)))
+# # coefficient matrix between quaternion rate and angular velocity in the global frame
+# RARM_Matrix_angular2quat = optas.casadi.SX(np.zeros((4, self.T_MPC * 3)))
+# LARM_Matrix_angular2quat = optas.casadi.SX(np.zeros((4, self.T_MPC * 3)))
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P[:, j]
+ name = 'control_point_' + str(i) + '_bound' # add position constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=lower, mid=P[:, i], rhs=upper)
+# builder_wholebodyMPC.add_equality_constraint('twoarm_miniscope' + str(i), q_var_MPC[6, i], rhs=-q_var_MPC[12, i])
+
+# ori_function_R[:, i] = ori_fnc_Right(q_var_MPC[:, i])
+# ori_function_L[:, i] = ori_fnc_Left(q_var_MPC[:, i])
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,0] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,1] = ori_function_R[:, i][2]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,2] = ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,0] = -ori_function_R[:, i][2]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,1] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,2] = ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,0] = ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,1] = -ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,2] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,0] = -ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,1] = -ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,2] = -ori_function_R[:, i][2]
+
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,0] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,1] = ori_function_L[:, i][2]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,2] = ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,0] = -ori_function_L[:, i][2]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,1] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,2] = ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,0] = ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,1] = -ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,2] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,0] = -ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,1] = -ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,2] = -ori_function_L[:, i][2]
+
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm position' + str(i), optas.sumsqr(pos_fnc_Right(q_var_MPC[:, i])-pos_R[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position' + str(i), optas.sumsqr(pos_fnc_Left(q_var_MPC[:, i])-pos_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i]+q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.05 * optas.sumsqr(P[:, i+1] - P[:, i]))
+# if(i<(self.T_MPC -1)):
+# builder_wholebodyMPC.add_cost_term('minimize_velocity2' + str(i), 0.0005 * optas.sumsqr(q_var_MPC[6:self.ndof, i+1] - q_var_MPC[6:self.ndof, i]))
+
+# obstacle_pos = np.asarray([[3.8], [0]])
+# obstacle_radius = 0.75
+# offset = np.asarray([[0.5], [0]])
+# builder_wholebodyMPC.add_geq_inequality_constraint('donkey_obstacle' + str(i), lhs=optas.norm_2(P[0:2, i] + offset - obstacle_pos), rhs=obstacle_radius**2)
+
+# if(i<(self.T_MPC -1)):
+# builder_wholebodyMPC.add_cost_term('Length' + str(i), optas.sumsqr(P[:, i+1]-P[:, i]))
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', P[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', P[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', P[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+
+# builder_wholebodyMPC.add_bound_inequality_constraint('init_p_equal', lhs=init_position_MPC[3:self.ndof] - np.full((15, 1), 0.005), mid=P[3:self.ndof, 0] + p_epsilon[3:self.ndof], rhs=init_position_MPC[3:self.ndof] + np.full((15, 1), 0.005))
+# builder_wholebodyMPC.add_bound_inequality_constraint('init_p_equal2', lhs=init_position_MPC[0:3] - np.full((3, 1), 0.001), mid=P[0:3, 0] + p_epsilon[0:3], rhs=init_position_MPC[0:3] + np.full((3, 1), 0.001))
+# builder_wholebodyMPC.add_cost_term('minimize_p_epsilon', optas.sumsqr(p_epsilon))
+
+# builder_wholebodyMPC.add_inequality_constraint('init_velocity', lhs=self.n * (P[0:3, 1] - P[0:3, 0]), rhs=init_velocity_MPC[0:3])
+
+
+# # get end-effector pose as parameters
+# dpos_R = builder_wholebodyMPC.add_parameter('dpos_R', 3, self.T_MPC)
+# dori_R = builder_wholebodyMPC.add_parameter('dori_R', 4, self.T_MPC)
+# dpos_L = builder_wholebodyMPC.add_parameter('dpos_L', 3, self.T_MPC)
+# dori_L = builder_wholebodyMPC.add_parameter('dori_L', 4, self.T_MPC)
+# # functions of right and left arm positions
+# dpos_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+# dpos_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+# # quaternion functions of two arm end effectors
+# dori_fnc_Right = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_right)
+# dori_fnc_Left = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_left)
+ # define dq function depending on P
+
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0001/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P[:, j+1] - P[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=self.n * (P[:, i+1] - P[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_L[:, i]))
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P[:, j+2] - 2*P[:, j+1] + P[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+# # optimization cost: close to target
+# builder_wholebodyMPC.add_cost_term('Right_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_L[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm angular velocity' + str(i), optas.sumsqr(0.5 * RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3] @ dori_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dori_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm angular velocity' + str(i), optas.sumsqr(0.5 * LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3] @ dori_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i] -dori_L[:, i]))
+
+
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('ipopt', solver_options={'ipopt.print_level': 0, 'print_time': 0})
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0, 'print_time': 0,
+ 'knitro.FeasTol': 1e-6, 'knitro.OptTol': 1e-6, 'knitro.ftol':1e-6,
+ 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1} )
+ self.ti_MPC = 0 # time index of the MPC
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ ### ---------------------------------------------------------
+ # declare ft_sensor subscriber
+ self._ft_right_sub = rospy.Subscriber(
+ "/ft_right/raw/data",
+ WrenchStamped,
+ self.read_ft_sensor_right_data_cb
+ )
+ self._ft_left_sub = rospy.Subscriber(
+ "/ft_left/raw/data",
+ WrenchStamped,
+ self.read_ft_sensor_left_data_cb
+ )
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/chonk/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+# self._joint_sub_base = rospy.Subscriber(
+# "/chonk/donkey_velocity_controller/odom",
+# Odometry,
+# self.read_base_states_cb
+# )
+ self._joint_sub_base = rospy.Subscriber(
+ "/chonk/base_pose_ground_truth",
+ Odometry,
+ self.read_base_states_cb
+ )
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher(
+ self._pub_cmd_topic_name,
+ Float64MultiArray,
+ queue_size=10
+ )
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher(
+ "/chonk/donkey_velocity_controller/cmd_vel",
+ Twist,
+ queue_size=10
+ )
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ # initialize action messages
+ self._feedback = CmdChonkPoseFeedback()
+ self._result = CmdChonkPoseResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'cmd_pose',
+ CmdChonkPoseAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([
+ acceped_goal.poseR.position.x,
+ acceped_goal.poseR.position.y,
+ acceped_goal.poseR.position.z
+ ])
+ pos_Left = np.asarray([
+ acceped_goal.poseL.position.x,
+ acceped_goal.poseL.position.y,
+ acceped_goal.poseL.position.z
+ ])
+ ori_Right = np.asarray([
+ acceped_goal.poseR.orientation.x,
+ acceped_goal.poseR.orientation.y,
+ acceped_goal.poseR.orientation.z,
+ acceped_goal.poseR.orientation.w
+ ])
+ ori_Left = np.asarray([
+ acceped_goal.poseL.orientation.x,
+ acceped_goal.poseL.orientation.y,
+ acceped_goal.poseL.orientation.z,
+ acceped_goal.poseL.orientation.w
+ ])
+ # check boundaries of the position
+ if (pos_Right > self._pos_max).any() or (pos_Right < self._pos_min).any():
+ rospy.logwarn("%s: Request aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Right[0], pos_Right[1], pos_Right[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ if (pos_Left > self._pos_max).any() or (pos_Left < self._pos_min).any():
+ rospy.logwarn("%s: Lequest aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Left[0], pos_Left[1], pos_Left[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name,
+ pos_Right[0], pos_Right[1], pos_Right[2],
+ ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2],
+ ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3],
+ acceped_goal.duration
+ )
+ )
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### optas
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+
+# print(np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=np.zeros(18))).T[0])
+# print(np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=np.zeros(18))).T[0])
+# print(np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=np.zeros(18))).T[0])
+# print(np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=np.zeros(18))).T[0])
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - start_RARM_pos
+ Derivation_RARM_Quat = ori_Right - start_RARM_quat
+ Derivation_LARM_Pos = pos_Left - start_LARM_pos
+ Derivation_LARM_Quat = ori_Left - start_LARM_quat
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._DRARM_ee_Pos_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_RARM_Pos/self.duration)
+ self._LARM_ee_Pos_trajectory = lambda t: start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._DLARM_ee_Pos_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_LARM_Pos/self.duration)
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._DRARM_ee_Quat_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_RARM_Quat/self.duration)
+ self._LARM_ee_Quat_trajectory = lambda t: start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ self._DLARM_ee_Quat_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_LARM_Quat/self.duration)
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ """Sensor data in ee frame"""
+ ft_ee_right = self.X_ee_INsensorFrame_Right.T @ self.ft_right
+ ft_ee_left = self.X_ee_INsensorFrame_Left.T @ self.ft_left
+# """ee mass data in ee frame"""
+# ft_ee_mass_right = X_fromRandP_different(self.rot_ee_right_fnc_global(self.q_curr), self.pos_mass_INee_Right).T @ self.mass_ee_Force
+# ft_ee_mass_left = X_fromRandP_different(self.rot_ee_left_fnc_global(self.q_curr), self.pos_mass_INee_Left).T @ self.mass_ee_Force
+# """Sensor data delete mass influence"""
+# ft_ee_composite_right = ft_ee_right + ft_ee_mass_right
+# ft_ee_composite_left = ft_ee_left + ft_ee_mass_left
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+
+ pos_R_goal = []
+ ori_R_goal = []
+ pos_L_goal = []
+ ori_L_goal = []
+ dpos_R_goal = []
+ dori_R_goal = []
+ dpos_L_goal = []
+ dori_L_goal = []
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+# print(g_rarm_ee_ori[0]**2+g_rarm_ee_ori[1]**2+g_rarm_ee_ori[2]**2 +g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+
+# dg_rarm_ee_pos = self._DRARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# dpos_R_goal.append(dg_rarm_ee_pos.tolist())
+# dg_rarm_ee_ori = self._DRARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# dori_R_goal.append(dg_rarm_ee_ori.tolist())
+# dg_larm_ee_pos = self._DLARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# dpos_L_goal.append(dg_larm_ee_pos.tolist())
+# dg_larm_ee_ori = self._DLARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# dori_L_goal.append(dg_larm_ee_ori.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+# dpos_R_goal.append(dg_rarm_ee_pos.tolist()) # i.e. previous goal
+# dori_R_goal.append(dg_rarm_ee_ori.tolist()) # i.e. previous goal
+# dpos_L_goal.append(dg_rarm_ee_pos.tolist()) # i.e. previous goal
+# dori_L_goal.append(dg_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+# dpos_R_goal = optas.np.array(dpos_R_goal).T
+# dori_R_goal = optas.np.array(dori_R_goal).T
+# dpos_L_goal = optas.np.array(dpos_L_goal).T
+# dori_L_goal = optas.np.array(dori_L_goal).T
+
+
+
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'P': self.curr_MPC})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'P': self.solution_MPC[f'P']})
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal, 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr } )
+# self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal, 't': self.timebyT} )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ P = np.asarray(self.solution_MPC[f'P'])
+
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC
+ n = self.T_MPC -1
+ q_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * P[:, j]
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P[:, j+1] - P[:, j])
+# q_next = P[:, 0]
+# dq_next = self.n * (P[:, 1] - P[:, 0])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+# self._msg.data = [float('%.3f' % x) for x in self._msg.data]
+# print(self._msg.data)
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_ft_sensor_right_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_right = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_ft_sensor_left_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_left = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+# base_linear_velocity_global = self.donkey_R @ self.donkey_velocity
+# base_angular_velocity_global = self.donkey_R @ self.donkey_angular_velocity
+# self.dq_curr_base = [base_linear_velocity_global[0], base_linear_velocity_global[1], base_angular_velocity_global[2]]
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_AD_obstable_pick.py b/script/action_server_cmd_pose_MPC_BC_operational_AD_obstable_pick.py
new file mode 100755
index 0000000..b2b7542
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_AD_obstable_pick.py
@@ -0,0 +1,785 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+ self.dt_MPC = 0.12 # time step
+ self.T_MPC = 8 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_Right = builder_wholebodyMPC_planner.add_decision_variables('R_Right', 3, self.T_MPC)
+ R_Left = builder_wholebodyMPC_planner.add_decision_variables('R_Left', 3, self.T_MPC)
+ R_middle = builder_wholebodyMPC_planner.add_decision_variables('R_middle', 3, self.T_MPC)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC)
+
+ t = builder_wholebodyMPC_planner.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+# init_r_position_middle = builder_wholebodyMPC_planner.add_parameter('init_r_position_middle', 2)
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 2)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 2)
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3, self.T_MPC)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3, self.T_MPC)
+ ori_R = builder_wholebodyMPC_planner.add_parameter('ori_R', 4, self.T_MPC)
+ ori_L = builder_wholebodyMPC_planner.add_parameter('ori_L', 4, self.T_MPC)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ r_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ r_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ r_middle_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * R_middle[:, j]
+ r_RARM_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * R_Right[:, j]
+ r_LARM_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * R_Left[:, j]
+ # optimization cost: close to target
+ builder_wholebodyMPC_planner.add_cost_term('Right_middle_x' + str(i), optas.sumsqr(r_middle_var_MPC[0, i] - 0.5*(pos_R[0, i] + pos_L[0, i])))
+ builder_wholebodyMPC_planner.add_cost_term('Right_middle_y' + str(i), 100*optas.sumsqr(r_middle_var_MPC[1, i] - 0.5*(pos_R[1, i] + pos_L[1, i])))
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - r_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_RARM_var_MPC[0, i], rhs=r_LARM_var_MPC[0, i])
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_middle_z', r_middle_var_MPC[2, :], rhs=0.5*(pos_R[2, :]+pos_L[2, :]))
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_xy', r_RARM_var_MPC[0:2, :], rhs=r_middle_var_MPC[0:2, :] - 0.5*(pos_L[0:2, :]-pos_R[0:2, :]))
+ builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_xy', r_LARM_var_MPC[0:2, :], rhs=r_middle_var_MPC[0:2, :] + 0.5*(pos_L[0:2, :]-pos_R[0:2, :]))
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_z', r_RARM_var_MPC[2, :], rhs=r_middle_var_MPC[2, :])
+ builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_z', r_LARM_var_MPC[2, :], rhs=r_middle_var_MPC[2, :])
+# builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_z', R_Right[2, :], rhs=pos_R[2, :])
+# builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_z', R_Left[2, :], rhs=pos_L[2, :])
+
+ for i in range(self.T_MPC):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+
+# builder_wholebodyMPC_planner.add_geq_inequality_constraint('RARM_ee_obstacle' + str(i), lhs=(r_RARM_var_MPC[0:2, i]-obstacle_pos).T @ (r_RARM_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius_right**2 + r_right_ep[i])
+# builder_wholebodyMPC_planner.add_geq_inequality_constraint('LARM_ee_obstacle' + str(i), lhs=(r_LARM_var_MPC[0:2, i]-obstacle_pos).T @ (r_LARM_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius_left**2 + r_left_ep[i])
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', optas.sumsqr(r_ep))
+
+# builder_wholebodyMPC_planner.add_cost_term('minimize_r_right_ep', optas.sumsqr(r_right_ep))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_r_left_ep', optas.sumsqr(r_left_ep))
+
+ # add position constraint at the beginning state
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[0:2, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_Right', R_Right[0:2, 0], rhs=init_r_position_Right)
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_Left', R_Left[0:2, 0], rhs=init_r_position_Left)
+
+
+ dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+# dr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+# dr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+
+ w_dr = 0.0001/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dr_middle_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (R_middle[:, j+1] - R_middle[:, j])
+# dr_RARM_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (R_Right[:, j+1] - R_Right[:, j])
+# dr_LARM_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (R_Left[:, j+1] - R_Left[:, j])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_right' + str(i), w_dq * optas.sumsqr(dr_RARM_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_left' + str(i), w_dq * optas.sumsqr(dr_LARM_var_MPC[:, i]))
+
+ ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ w_ddr = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddr_middle_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+# ddr_RARM_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (R_Right[:, j+2] - 2*R_Right[:, j+1] + R_Right[:, j])
+# ddr_LARM_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (R_Left[:, j+2] - 2*R_Left[:, j+1] + R_Left[:, j])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_right' + str(i), w_ddr * optas.sumsqr(ddr_RARM_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_left' + str(i), w_ddr * optas.sumsqr(ddr_LARM_var_MPC[:, i]))
+
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0, 'print_time': 0,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5, 'knitro.ftol':1e-6,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyMPC_LIMITS'
+ )
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.12 # time step
+ self.T_MPC = 8 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+
+ self.Lambda_ee_Right = np.diag([2, 2, 2])
+ self.Lambda_ee_Left = np.diag([2, 2, 2])
+ self.Lambda_ee_1DOF_Right = 2
+ self.Lambda_ee_1DOF_Left = 2
+ self.K_Right = np.diag([1000, 1000, 1000]) # Stiffness Right
+ self.K_Left = np.diag([1000, 1000, 1000]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(2*self.K_Right[0,0]), 2 * np.sqrt(2*self.K_Right[1,1]), 2 * np.sqrt(2*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(2*self.K_Left[0,0]), 2 * np.sqrt(2*self.K_Left[1,1]), 2 * np.sqrt(2*self.K_Left[2,2])]) # Damping Left
+
+ # get end-effector pose as parameters
+ pos_R_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ pos_L_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ R_Right_reasonal = builder_wholebodyMPC.add_parameter('R_Right_reasonal', 3, self.T_MPC)
+ R_Left_reasonal = builder_wholebodyMPC.add_parameter('R_Left_reasonal', 3, self.T_MPC)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+ F_ext_Right_target = builder_wholebodyMPC.add_parameter('F_ext_Right_target', 3, self.T_MPC)
+ F_ext_Left_target = builder_wholebodyMPC.add_parameter('F_ext_Left_target', 3, self.T_MPC)
+
+ # functions of right and left arm positions
+ pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+
+ pos_ee_INsensorFrame_Right = np.asarray([-0.08, 0, 0.039+0.04])
+ pos_ee_INsensorFrame_Left = np.asarray([-0.08, 0, 0.039+0.04])
+ rot_ee_INsensorFrame_Right = optas.spatialmath.roty(-np.pi/2)
+ rot_ee_INsensorFrame_Left = optas.spatialmath.roty(-np.pi/2)
+
+ self.X_ee_INsensorFrame_Right = X_fromRandP(rot_ee_INsensorFrame_Right, pos_ee_INsensorFrame_Right)
+ self.X_ee_INsensorFrame_Left = X_fromRandP(rot_ee_INsensorFrame_Left, pos_ee_INsensorFrame_Left)
+
+ self.rot_ee_right_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rot_ee_left_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ self.pos_mass_INee_Right = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+ self.pos_mass_INee_Left = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+
+ self.mass_ee_Force = np.asarray([0,0,0,0,0,-0.3113*9.8])
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ pos_R_reasonal[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * R_Right_reasonal[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * R_Left_reasonal[:, j]
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ F_ext_Right_var_MPC[:, i] = inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+
+
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - Delta_p_Left_var_MPC[:, i]))
+
+ builder_wholebodyMPC.add_cost_term('Right_arm Force' + str(i), optas.sumsqr(F_ext_Right_var_MPC[:, i] - F_ext_Right_target[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force' + str(i), optas.sumsqr(F_ext_Left_var_MPC[:, i] - F_ext_Left_target[:, i]))
+
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.01 * optas.sumsqr(q_var_MPC[6, i]+q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('head_miniscope' + str(i), 0.01 * optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.001 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[2, :], rhs=np.zeros((1, self.T_MPC)))
+
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint', P_Right[:, 0], rhs=init_Delta_position_Right)
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint', P_Left[:, 0], rhs=init_Delta_position_Right)
+
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0001/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_deriv_' + str(i) + '_bound', lhs=dlower, mid=self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity_dq' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+
+ # setup solver
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0, 'print_time': 0,
+ 'knitro.FeasTol': 1e-4, 'knitro.OptTol': 1e-4, 'knitro.ftol':1e-6,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+ 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3)
+# self.start_RARM_torque = np.zeros(3)
+ self.start_LARM_force = np.zeros(3)
+# self.start_LARM_torque = np.zeros(3)
+ ### ---------------------------------------------------------
+ # declare ft_sensor subscriber
+ self._ft_right_sub = rospy.Subscriber("/ft_right/raw/data", WrenchStamped, self.read_ft_sensor_right_data_cb)
+ self._ft_left_sub = rospy.Subscriber("/ft_left/raw/data", WrenchStamped, self.read_ft_sensor_left_data_cb)
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/streaming_controller/command", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp inertia publisher
+ self._inertia_sub = rospy.Subscriber("/chonk/arm_ee_inertias", Float64MultiArray, self.read_two_ee_grasp_inertia_data_cb)
+ self._inertia_1DOF_sub = rospy.Subscriber("/chonk/arm_ee_1DOF_inertias_1DOF", Float64MultiArray, self.read_two_ee_grasp_inertia_1DOF_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+# torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+# torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, pos_Right[0], pos_Right[1], pos_Right[2], ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2], ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - self.start_RARM_pos
+ Derivation_RARM_Quat = ori_Right - self.start_RARM_quat
+ Derivation_LARM_Pos = pos_Left - self.start_LARM_pos
+ Derivation_LARM_Quat = ori_Left - self.start_LARM_quat
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force
+# Derivation_RARM_torque = torque_Right - self.start_RARM_torque
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force
+# Derivation_LARM_torque = torque_Left - self.start_LARM_torque
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+# self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+# self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+
+# self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+# self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+# self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+# self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ """Sensor data in ee frame"""
+ ft_ee_right = self.X_ee_INsensorFrame_Right.T @ self.ft_right
+ ft_ee_left = self.X_ee_INsensorFrame_Left.T @ self.ft_left
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self.ti_MPC = 0 # time index of the MPC
+ self._idx += 1
+ pos_R_goal = []
+ ori_R_goal = []
+ pos_L_goal = []
+ ori_L_goal = []
+ force_R_goal = []
+ force_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_actual_Right = np.array(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr))[0:2]
+ r_actual_Left = np.array(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr))[0:2]
+
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': 0.5*(pos_R_goal+pos_L_goal), f'R_Right': pos_R_goal, f'R_Left': pos_L_goal, f'r_ep': np.zeros(self.T_MPC) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_middle': self.solution_MPC_planner[f'R_middle'],
+ f'R_Right': self.solution_MPC_planner[f'R_Right'],
+ f'R_Left': self.solution_MPC_planner[f'R_Left'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'] })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal,
+ 'pos_L': pos_L_goal, 'ori_L': ori_L_goal,
+ 't': self.timebyT,
+ 'init_r_position_Right': r_actual_Right,
+ 'init_r_position_Left': r_actual_Left} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_Right = np.asarray(self.solution_MPC_planner[f'R_Right'])
+ R_Left = np.asarray(self.solution_MPC_planner[f'R_Left'])
+ ### -----------------------------------------------------------
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ Derivation_RARM_pos_start = np.zeros(3)
+ Derivation_LARM_pos_start = np.zeros(3)
+ Derivation_RARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1] - R_Right[1, 0]
+ Derivation_LARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1] - R_Left[1, 0]
+
+
+
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC,
+ f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)) })
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({ f'Q': self.solution_MPC[f'Q'],
+ f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'] })
+
+
+ self.solver_wholebodyMPC.reset_parameters({'ori_R_reasonal': ori_R_goal, 'ori_L_reasonal': ori_L_goal,
+ 't': self.timebyT,
+ 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_target': force_R_goal, 'F_ext_Left_target': force_L_goal,
+ 'inertia_Right': self.Lambda_ee_Right, 'inertia_Left': self.Lambda_ee_Left,
+ 'init_Delta_position_Right': Derivation_RARM_pos_start,
+ 'init_Delta_position_Left': Derivation_LARM_pos_start,
+ 'R_Right_reasonal': R_Right,
+ 'R_Left_reasonal': R_Left } )
+# self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal, 't': self.timebyT} )
+
+# print(self.Lambda_ee_Right)
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+# P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+# P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+# R_Right = np.asarray(self.solution_MPC[f'R_Right'])
+# R_Left = np.asarray(self.solution_MPC[f'R_Left'])
+
+
+# print('---------------------------------------------------------------------------------------------------------------------------')
+# print(P_Right[1,:])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC
+ n = self.T_MPC -1
+ q_next = np.zeros(self.ndof)
+ p_right = np.zeros(3)
+ dp_right = np.zeros(3)
+ ddp_right = np.zeros(3)
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+# for j in range(self.T_MPC):
+# p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+# for j in range(self.T_MPC-1):
+# dp_right += self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+# for j in range(self.T_MPC-2):
+# ddp_right += self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+# print(self.Lambda_ee_Right @ ddp_right + self.K_Right @ p_right + self.D_Right @ dp_right)
+
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_ft_sensor_right_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_right = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_ft_sensor_left_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_left = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+ def read_two_ee_grasp_inertia_data_cb(self, msg):
+ self.Lambda_ee_Right = np.asarray([ [msg.data[0], msg.data[1], msg.data[2]],
+ [msg.data[1], msg.data[3], msg.data[4]],
+ [msg.data[2], msg.data[4], msg.data[5]] ])
+ self.Lambda_ee_Left = np.asarray([ [msg.data[6], msg.data[7], msg.data[8]],
+ [msg.data[7], msg.data[9], msg.data[10]],
+ [msg.data[8], msg.data[10], msg.data[11]] ])
+
+
+ def read_two_ee_grasp_inertia_1DOF_data_cb(self, msg):
+ self.Lambda_ee_1DOF_Right = np.asarray([ msg.data[0] ])
+ self.Lambda_ee_1DOF_Left = np.asarray([ msg.data[1] ])
+ self.Lambda_ee_Right[1, 1] = msg.data[0]
+ self.Lambda_ee_Left[1, 1] = msg.data[1]
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_AD_obstable_pick_longhorizon.py b/script/action_server_cmd_pose_MPC_BC_operational_AD_obstable_pick_longhorizon.py
new file mode 100755
index 0000000..7c4de41
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_AD_obstable_pick_longhorizon.py
@@ -0,0 +1,797 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+ self.dt_MPC_planner = 0.12 # time step
+ self.T_MPC_planner = 10 # T is number of time steps
+ self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_Right = builder_wholebodyMPC_planner.add_decision_variables('R_Right', 3, self.T_MPC_planner)
+ R_Left = builder_wholebodyMPC_planner.add_decision_variables('R_Left', 3, self.T_MPC_planner)
+ R_middle = builder_wholebodyMPC_planner.add_decision_variables('R_middle', 3, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+
+ t = builder_wholebodyMPC_planner.add_parameter('t', self.T_MPC_planner) # time
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+ # Add parameters
+# init_r_position_middle = builder_wholebodyMPC_planner.add_parameter('init_r_position_middle', 2)
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 2)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 2)
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3, self.T_MPC_planner)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3, self.T_MPC_planner)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+ r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Right[:, j]
+ r_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Left[:, j]
+ # optimization cost: close to target
+ builder_wholebodyMPC_planner.add_cost_term('Right_middle_x' + str(i), optas.sumsqr(r_middle_var_MPC[0, i] - 0.5*(pos_R[0, i] + pos_L[0, i])))
+ builder_wholebodyMPC_planner.add_cost_term('Right_middle_y' + str(i), 10*optas.sumsqr(r_middle_var_MPC[1, i] - 0.5*(pos_R[1, i] + pos_L[1, i])))
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - r_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_RARM_var_MPC[0, i], rhs=r_LARM_var_MPC[0, i])
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_middle_z', r_middle_var_MPC[2, :], rhs=0.5*(pos_R[2, :]+pos_L[2, :]))
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_xy', r_RARM_var_MPC[0:2, :], rhs=r_middle_var_MPC[0:2, :] - 0.5*(pos_L[0:2, :]-pos_R[0:2, :]))
+ builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_xy', r_LARM_var_MPC[0:2, :], rhs=r_middle_var_MPC[0:2, :] + 0.5*(pos_L[0:2, :]-pos_R[0:2, :]))
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_z', r_RARM_var_MPC[2, :], rhs=r_middle_var_MPC[2, :])
+ builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_z', r_LARM_var_MPC[2, :], rhs=r_middle_var_MPC[2, :])
+# builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_z', R_Right[2, :], rhs=pos_R[2, :])
+# builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_z', R_Left[2, :], rhs=pos_L[2, :])
+
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+
+# builder_wholebodyMPC_planner.add_geq_inequality_constraint('RARM_ee_obstacle' + str(i), lhs=(r_RARM_var_MPC[0:2, i]-obstacle_pos).T @ (r_RARM_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius_right**2 + r_right_ep[i])
+# builder_wholebodyMPC_planner.add_geq_inequality_constraint('LARM_ee_obstacle' + str(i), lhs=(r_LARM_var_MPC[0:2, i]-obstacle_pos).T @ (r_LARM_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius_left**2 + r_left_ep[i])
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', optas.sumsqr(r_ep))
+
+# builder_wholebodyMPC_planner.add_cost_term('minimize_r_right_ep', optas.sumsqr(r_right_ep))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_r_left_ep', optas.sumsqr(r_left_ep))
+
+ # add position constraint at the beginning state
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[0:2, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_Right', R_Right[0:2, 0], rhs=init_r_position_Right)
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_Left', R_Left[0:2, 0], rhs=init_r_position_Left)
+
+
+ dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+# dr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+# dr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ w_dr = 0.0001/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+ dr_middle_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+# dr_RARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Right[:, j+1] - R_Right[:, j])
+# dr_LARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Left[:, j+1] - R_Left[:, j])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_right' + str(i), w_dq * optas.sumsqr(dr_RARM_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_left' + str(i), w_dq * optas.sumsqr(dr_LARM_var_MPC[:, i]))
+
+ ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ w_ddr = 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+ ddr_middle_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+# ddr_RARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Right[:, j+2] - 2*R_Right[:, j+1] + R_Right[:, j])
+# ddr_LARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Left[:, j+2] - 2*R_Left[:, j+1] + R_Left[:, j])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_right' + str(i), w_ddr * optas.sumsqr(ddr_RARM_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_left' + str(i), w_ddr * optas.sumsqr(ddr_LARM_var_MPC[:, i]))
+
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0, 'print_time': 0,
+ 'knitro.FeasTol': 5e-4, 'knitro.OptTol': 5e-4, 'knitro.ftol':1e-6,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+ self.time_linspace_planner = np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner)
+ self.timebyT_planner = np.asarray(self.time_linspace_planner)/self.duration_MPC_planner
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyMPC_LIMITS'
+ )
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.12 # time step
+ self.T_MPC = 8 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+
+ self.Lambda_ee_Right = np.diag([2, 2, 2])
+ self.Lambda_ee_Left = np.diag([2, 2, 2])
+ self.Lambda_ee_1DOF_Right = 2
+ self.Lambda_ee_1DOF_Left = 2
+ self.K_Right = np.diag([1000, 1000, 1000]) # Stiffness Right
+ self.K_Left = np.diag([1000, 1000, 1000]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(2*self.K_Right[0,0]), 2 * np.sqrt(2*self.K_Right[1,1]), 2 * np.sqrt(2*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(2*self.K_Left[0,0]), 2 * np.sqrt(2*self.K_Left[1,1]), 2 * np.sqrt(2*self.K_Left[2,2])]) # Damping Left
+
+ # get end-effector pose as parameters
+ pos_R_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ pos_L_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ R_Right_reasonal = builder_wholebodyMPC.add_parameter('R_Right_reasonal', 3, self.T_MPC_planner)
+ R_Left_reasonal = builder_wholebodyMPC.add_parameter('R_Left_reasonal', 3, self.T_MPC_planner)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+ F_ext_Right_target = builder_wholebodyMPC.add_parameter('F_ext_Right_target', 3, self.T_MPC)
+ F_ext_Left_target = builder_wholebodyMPC.add_parameter('F_ext_Left_target', 3, self.T_MPC)
+
+ # functions of right and left arm positions
+ pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+
+ pos_ee_INsensorFrame_Right = np.asarray([-0.08, 0, 0.039+0.04])
+ pos_ee_INsensorFrame_Left = np.asarray([-0.08, 0, 0.039+0.04])
+ rot_ee_INsensorFrame_Right = optas.spatialmath.roty(-np.pi/2)
+ rot_ee_INsensorFrame_Left = optas.spatialmath.roty(-np.pi/2)
+
+ self.X_ee_INsensorFrame_Right = X_fromRandP(rot_ee_INsensorFrame_Right, pos_ee_INsensorFrame_Right)
+ self.X_ee_INsensorFrame_Left = X_fromRandP(rot_ee_INsensorFrame_Left, pos_ee_INsensorFrame_Left)
+
+ self.rot_ee_right_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rot_ee_left_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ self.pos_mass_INee_Right = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+ self.pos_mass_INee_Left = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+
+ self.mass_ee_Force = np.asarray([0,0,0,0,0,-0.3113*9.8])
+
+ fac = self.duration_MPC/self.duration_MPC_planner
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t[i]*fac)**j * (1-t[i]*fac)**(self.n_planner-j) * R_Right_reasonal[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t[i]*fac)**j * (1-t[i]*fac)**(self.n_planner-j) * R_Left_reasonal[:, j]
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ F_ext_Right_var_MPC[:, i] = inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+
+
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - Delta_p_Left_var_MPC[:, i]))
+
+ builder_wholebodyMPC.add_cost_term('Right_arm Force' + str(i), optas.sumsqr(F_ext_Right_var_MPC[:, i] - F_ext_Right_target[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force' + str(i), optas.sumsqr(F_ext_Left_var_MPC[:, i] - F_ext_Left_target[:, i]))
+
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.01 * optas.sumsqr(q_var_MPC[6, i]+q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('head_miniscope' + str(i), 0.01 * optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.002 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.001 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[2, :], rhs=np.zeros((1, self.T_MPC)))
+
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint', P_Right[:, 0], rhs=init_Delta_position_Right)
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint', P_Left[:, 0], rhs=init_Delta_position_Right)
+
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0001/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_deriv_' + str(i) + '_bound', lhs=dlower, mid=self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity_dq' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+
+ # setup solver
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0, 'print_time': 0,
+ 'knitro.FeasTol': 1e-4, 'knitro.OptTol': 1e-4, 'knitro.ftol':1e-6,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+ 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3)
+# self.start_RARM_torque = np.zeros(3)
+ self.start_LARM_force = np.zeros(3)
+# self.start_LARM_torque = np.zeros(3)
+ ### ---------------------------------------------------------
+ # declare ft_sensor subscriber
+ self._ft_right_sub = rospy.Subscriber("/ft_right/raw/data", WrenchStamped, self.read_ft_sensor_right_data_cb)
+ self._ft_left_sub = rospy.Subscriber("/ft_left/raw/data", WrenchStamped, self.read_ft_sensor_left_data_cb)
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/streaming_controller/command", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp inertia publisher
+ self._inertia_sub = rospy.Subscriber("/chonk/arm_ee_inertias", Float64MultiArray, self.read_two_ee_grasp_inertia_data_cb)
+ self._inertia_1DOF_sub = rospy.Subscriber("/chonk/arm_ee_1DOF_inertias_1DOF", Float64MultiArray, self.read_two_ee_grasp_inertia_1DOF_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+# torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+# torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, pos_Right[0], pos_Right[1], pos_Right[2], ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2], ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - self.start_RARM_pos
+ Derivation_RARM_Quat = ori_Right - self.start_RARM_quat
+ Derivation_LARM_Pos = pos_Left - self.start_LARM_pos
+ Derivation_LARM_Quat = ori_Left - self.start_LARM_quat
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force
+# Derivation_RARM_torque = torque_Right - self.start_RARM_torque
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force
+# Derivation_LARM_torque = torque_Left - self.start_LARM_torque
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+# self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+# self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+
+# self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+# self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+# self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+# self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ """Sensor data in ee frame"""
+# ft_ee_right = self.X_ee_INsensorFrame_Right.T @ self.ft_right
+# ft_ee_left = self.X_ee_INsensorFrame_Left.T @ self.ft_left
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self.ti_MPC_planner = 0 # time index of the MPC
+ self._idx += 1
+ pos_R_goal = []
+ pos_L_goal = []
+
+ for i in range(self.T_MPC_planner):
+ if(self.ti_MPC_planner <= self.duration):
+ self.ti_MPC_planner = self._t[self._idx-1] + self.dt_MPC_planner*i
+ if(self.ti_MPC_planner > self.duration):
+ self.ti_MPC_planner = self.duration
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC_planner).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC_planner).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_actual_Right = np.array(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr))[0:2]
+ r_actual_Left = np.array(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr))[0:2]
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_middle': 0.5*(pos_R_goal+pos_L_goal), f'R_Right': pos_R_goal,
+ f'R_Left': pos_L_goal, f'r_ep': np.zeros(self.T_MPC_planner) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_middle': self.solution_MPC_planner[f'R_middle'],
+ f'R_Right': self.solution_MPC_planner[f'R_Right'],
+ f'R_Left': self.solution_MPC_planner[f'R_Left'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'] })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': pos_R_goal, 'pos_L': pos_L_goal,
+ 't': self.timebyT_planner,
+ 'init_r_position_Right': r_actual_Right,
+ 'init_r_position_Left': r_actual_Left} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_Right = np.asarray(self.solution_MPC_planner[f'R_Right'])
+ R_Left = np.asarray(self.solution_MPC_planner[f'R_Left'])
+ ### -----------------------------------------------------------
+ self.ti_MPC = 0
+ force_R_goal = []
+ force_L_goal = []
+ ori_R_goal = []
+ ori_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC_planner).flatten()
+ g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC_planner).flatten()
+ g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ Derivation_RARM_pos_start = np.zeros(3)
+ Derivation_LARM_pos_start = np.zeros(3)
+ Derivation_RARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1] - R_Right[1, 0]
+ Derivation_LARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1] - R_Left[1, 0]
+
+
+
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC,
+ f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)) })
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({ f'Q': self.solution_MPC[f'Q'],
+ f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'] })
+
+
+ self.solver_wholebodyMPC.reset_parameters({'ori_R_reasonal': ori_R_goal, 'ori_L_reasonal': ori_L_goal,
+ 't': self.timebyT,
+ 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_target': force_R_goal, 'F_ext_Left_target': force_L_goal,
+ 'inertia_Right': self.Lambda_ee_Right, 'inertia_Left': self.Lambda_ee_Left,
+ 'init_Delta_position_Right': Derivation_RARM_pos_start,
+ 'init_Delta_position_Left': Derivation_LARM_pos_start,
+ 'R_Right_reasonal': R_Right,
+ 'R_Left_reasonal': R_Left } )
+# self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal, 't': self.timebyT} )
+
+# print(self.Lambda_ee_Right)
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+# P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+# P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+# R_Right = np.asarray(self.solution_MPC[f'R_Right'])
+# R_Left = np.asarray(self.solution_MPC[f'R_Left'])
+
+
+# print('---------------------------------------------------------------------------------------------------------------------------')
+# print(P_Right[1,:])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC
+ n = self.T_MPC -1
+ q_next = np.zeros(self.ndof)
+ p_right = np.zeros(3)
+ dp_right = np.zeros(3)
+ ddp_right = np.zeros(3)
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+# for j in range(self.T_MPC):
+# p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+# for j in range(self.T_MPC-1):
+# dp_right += self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+# for j in range(self.T_MPC-2):
+# ddp_right += self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+# print(self.Lambda_ee_Right @ ddp_right + self.K_Right @ p_right + self.D_Right @ dp_right)
+
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_ft_sensor_right_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_right = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_ft_sensor_left_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_left = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+ def read_two_ee_grasp_inertia_data_cb(self, msg):
+ self.Lambda_ee_Right = np.asarray([ [msg.data[0], msg.data[1], msg.data[2]],
+ [msg.data[1], msg.data[3], msg.data[4]],
+ [msg.data[2], msg.data[4], msg.data[5]] ])
+ self.Lambda_ee_Left = np.asarray([ [msg.data[6], msg.data[7], msg.data[8]],
+ [msg.data[7], msg.data[9], msg.data[10]],
+ [msg.data[8], msg.data[10], msg.data[11]] ])
+
+
+ def read_two_ee_grasp_inertia_1DOF_data_cb(self, msg):
+ self.Lambda_ee_1DOF_Right = np.asarray([ msg.data[0] ])
+ self.Lambda_ee_1DOF_Left = np.asarray([ msg.data[1] ])
+ self.Lambda_ee_Right[1, 1] = msg.data[0]
+ self.Lambda_ee_Left[1, 1] = msg.data[1]
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_AD_pick.py b/script/action_server_cmd_pose_MPC_BC_operational_AD_pick.py
new file mode 100755
index 0000000..ace065b
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_AD_pick.py
@@ -0,0 +1,627 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyMPC_LIMITS'
+ )
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+# p_epsilon = builder_wholebodyMPC.add_decision_variables('p_epsilon', self.ndof)
+
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+
+ self.Lambda_ee_Right = np.diag([2, 2, 2])
+ self.Lambda_ee_Left = np.diag([2, 2, 2])
+ self.Lambda_ee_1DOF_Right = 2
+ self.Lambda_ee_1DOF_Left = 2
+ self.K_Right = np.diag([1000, 1000, 1000]) # Stiffness Right
+ self.K_Left = np.diag([1000, 1000, 1000]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(2*self.K_Right[0,0]), 2 * np.sqrt(2*self.K_Right[1,1]), 2 * np.sqrt(2*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(2*self.K_Left[0,0]), 2 * np.sqrt(2*self.K_Left[1,1]), 2 * np.sqrt(2*self.K_Left[2,2])]) # Damping Left
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+ ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+ pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+ ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+ F_ext_Right_target = builder_wholebodyMPC.add_parameter('F_ext_Right_target', 3, self.T_MPC)
+ F_ext_Left_target = builder_wholebodyMPC.add_parameter('F_ext_Left_target', 3, self.T_MPC)
+
+ # functions of right and left arm positions
+ pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+# F_ext_Right_upper = builder_wholebodyMPC.add_parameter('F_ext_Right_upper', 3)
+# F_ext_Left_upper = builder_wholebodyMPC.add_parameter('F_ext_Left_upper', 3)
+
+ pos_ee_INsensorFrame_Right = np.asarray([-0.08, 0, 0.039+0.04])
+ pos_ee_INsensorFrame_Left = np.asarray([-0.08, 0, 0.039+0.04])
+ rot_ee_INsensorFrame_Right = optas.spatialmath.roty(-np.pi/2)
+ rot_ee_INsensorFrame_Left = optas.spatialmath.roty(-np.pi/2)
+
+ self.X_ee_INsensorFrame_Right = X_fromRandP(rot_ee_INsensorFrame_Right, pos_ee_INsensorFrame_Right)
+ self.X_ee_INsensorFrame_Left = X_fromRandP(rot_ee_INsensorFrame_Left, pos_ee_INsensorFrame_Left)
+
+ self.rot_ee_right_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rot_ee_left_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ self.pos_mass_INee_Right = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+ self.pos_mass_INee_Left = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+
+ self.mass_ee_Force = np.asarray([0,0,0,0,0,-0.3113*9.8])
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+
+ F_ext_Right_var_MPC[:, i] = inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ name = 'control_point_' + str(i) + '_bound' # add position constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=lower, mid=Q[:, i], rhs=upper)
+# builder_wholebodyMPC.add_bound_inequality_constraint("Force_Right_" + str(i) + '_bound', lhs=np.zeros((3,1)), mid=F_ext_Right_var_MPC[:, i], rhs=np.full((3,1), 20))
+# builder_wholebodyMPC.add_bound_inequality_constraint("Force_Left_" + str(i) + '_bound', lhs=np.zeros((3,1)), mid=F_ext_Left_var_MPC[:, i], rhs=np.full((3,1), 20))
+# builder_wholebodyMPC.add_bound_inequality_constraint("P_Right_" + str(i) + '_bound', lhs=np.full((3,1), -0.15), mid=P_Right[:, i], rhs=np.full((3,1), 0.15))
+# builder_wholebodyMPC.add_bound_inequality_constraint("P_Left_" + str(i) + '_bound', lhs=np.full((3,1), -0.15), mid=P_Left[:, i], rhs=np.full((3,1), 0.15))
+
+ # optimization cost: close to target
+# builder_wholebodyMPC.add_cost_term('Right_arm position' + str(i), optas.sumsqr(pos_fnc_Right(q_var_MPC[:, i])-pos_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm position' + str(i), optas.sumsqr(pos_fnc_Left(q_var_MPC[:, i])-pos_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(pos_fnc_Right(q_var_MPC[:, i])-pos_R[:, i] - Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(pos_fnc_Left(q_var_MPC[:, i])-pos_L[:, i] - Delta_p_Left_var_MPC[:, i]))
+
+ builder_wholebodyMPC.add_cost_term('Right_arm Force' + str(i), optas.sumsqr(F_ext_Right_var_MPC[:, i] - F_ext_Right_target[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force' + str(i), optas.sumsqr(F_ext_Left_var_MPC[:, i] - F_ext_Left_target[:, i]))
+
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.01 * optas.sumsqr(q_var_MPC[6, i]+q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('head_miniscope' + str(i), 0.01 * optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.001 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[2, :], rhs=np.zeros((1, self.T_MPC)))
+# builder_wholebodyMPC.add_equality_constraint('F_ext_Right_var_MPC_non_motion_direction_x', F_ext_Right_var_MPC[0, :], rhs=np.zeros((1, self.T_MPC)))
+# builder_wholebodyMPC.add_equality_constraint('F_ext_Right_var_MPC_non_motion_direction_z', F_ext_Right_var_MPC[2, :], rhs=np.zeros((1, self.T_MPC)))
+# builder_wholebodyMPC.add_equality_constraint('F_ext_Left_var_MPC_non_motion_direction_x', F_ext_Left_var_MPC[0, :], rhs=np.zeros((1, self.T_MPC)))
+# builder_wholebodyMPC.add_equality_constraint('F_ext_Left_var_MPC_non_motion_direction_z', F_ext_Left_var_MPC[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint', P_Right[:, 0], rhs=init_Delta_position_Right)
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint', P_Left[:, 0], rhs=init_Delta_position_Right)
+
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0001/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+
+ # setup solver
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0, 'print_time': 0,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5, 'knitro.ftol':1e-5, 'knitro.algorithm':1,
+ 'knitro.linsolver':2, 'knitro.maxtime_real': 1.8e-2, 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1} )
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3)
+# self.start_RARM_torque = np.zeros(3)
+ self.start_LARM_force = np.zeros(3)
+# self.start_LARM_torque = np.zeros(3)
+ ### ---------------------------------------------------------
+ # declare ft_sensor subscriber
+ self._ft_right_sub = rospy.Subscriber("/ft_right/raw/data", WrenchStamped, self.read_ft_sensor_right_data_cb)
+ self._ft_left_sub = rospy.Subscriber("/ft_left/raw/data", WrenchStamped, self.read_ft_sensor_left_data_cb)
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher(self._pub_cmd_topic_name, Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp inertia publisher
+ self._inertia_sub = rospy.Subscriber("/chonk/arm_ee_inertias", Float64MultiArray, self.read_two_ee_grasp_inertia_data_cb)
+ self._inertia_1DOF_sub = rospy.Subscriber("/chonk/arm_ee_1DOF_inertias_1DOF", Float64MultiArray, self.read_two_ee_grasp_inertia_1DOF_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+# torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+# torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, pos_Right[0], pos_Right[1], pos_Right[2], ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2], ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - self.start_RARM_pos
+ Derivation_RARM_Quat = ori_Right - self.start_RARM_quat
+ Derivation_LARM_Pos = pos_Left - self.start_LARM_pos
+ Derivation_LARM_Quat = ori_Left - self.start_LARM_quat
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force
+# Derivation_RARM_torque = torque_Right - self.start_RARM_torque
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force
+# Derivation_LARM_torque = torque_Left - self.start_LARM_torque
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+# self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+# self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+
+# self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+# self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+# self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+# self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ """Sensor data in ee frame"""
+ ft_ee_right = self.X_ee_INsensorFrame_Right.T @ self.ft_right
+ ft_ee_left = self.X_ee_INsensorFrame_Left.T @ self.ft_left
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self.ti_MPC = 0 # time index of the MPC
+ self._idx += 1
+ pos_R_goal = []
+ ori_R_goal = []
+ pos_L_goal = []
+ ori_L_goal = []
+ force_R_goal = []
+ force_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ Derivation_RARM_pos_start = np.zeros(3)
+ Derivation_LARM_pos_start = np.zeros(3)
+ Derivation_RARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1] - pos_R_goal[1, 0]
+ Derivation_LARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1] - pos_L_goal[1, 0]
+
+# print(Derivation_RARM_pos_start)
+# print(force_R_goal[1, :])
+
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC,
+ f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({ f'Q': self.solution_MPC[f'Q'],
+ f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'] })
+
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_target': force_R_goal, 'F_ext_Left_target': force_L_goal,
+# 'F_ext_Right_upper': self.force_Right, 'F_ext_Left_upper': self.force_Left,
+ 'inertia_Right': self.Lambda_ee_Right, 'inertia_Left': self.Lambda_ee_Left,
+ 'init_Delta_position_Right': Derivation_RARM_pos_start,
+ 'init_Delta_position_Left': Derivation_LARM_pos_start } )
+# self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal, 't': self.timebyT} )
+
+# print(self.Lambda_ee_Right)
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+
+
+# print('---------------------------------------------------------------------------------------------------------------------------')
+# print(P_Right[1,:])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC
+ n = self.T_MPC -1
+ q_next = np.zeros(self.ndof)
+ p_right = np.zeros(3)
+ dp_right = np.zeros(3)
+ ddp_right = np.zeros(3)
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ print(self.Lambda_ee_Right @ ddp_right + self.K_Right @ p_right + self.D_Right @ dp_right)
+
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_ft_sensor_right_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_right = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_ft_sensor_left_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_left = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+ def read_two_ee_grasp_inertia_data_cb(self, msg):
+ self.Lambda_ee_Right = np.asarray([ [msg.data[0], msg.data[1], msg.data[2]],
+ [msg.data[1], msg.data[3], msg.data[4]],
+ [msg.data[2], msg.data[4], msg.data[5]] ])
+ self.Lambda_ee_Left = np.asarray([ [msg.data[6], msg.data[7], msg.data[8]],
+ [msg.data[7], msg.data[9], msg.data[10]],
+ [msg.data[8], msg.data[10], msg.data[11]] ])
+
+
+ def read_two_ee_grasp_inertia_1DOF_data_cb(self, msg):
+ self.Lambda_ee_1DOF_Right = np.asarray([ msg.data[0] ])
+ self.Lambda_ee_1DOF_Left = np.asarray([ msg.data[1] ])
+ self.Lambda_ee_Right[1, 1] = msg.data[0]
+ self.Lambda_ee_Left[1, 1] = msg.data[1]
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_TC.py b/script/action_server_cmd_pose_MPC_BC_operational_TC.py
new file mode 100755
index 0000000..dcb2bfc
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_TC.py
@@ -0,0 +1,661 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseFeedback, CmdChonkPoseResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+# self._link_sensor_right = rospy.get_param('~link_sensor_right', 'link_sensor_right')
+# self._link_sensor_left = rospy.get_param('~link_sensor_left', 'link_sensor_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyMPC_LIMITS'
+ )
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 6 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ P = builder_wholebodyMPC.add_decision_variables('P', self.ndof, self.T_MPC)
+# p_epsilon = builder_wholebodyMPC.add_decision_variables('p_epsilon', self.ndof)
+
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+ ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+ pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+ ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+ # functions of right and left arm positions
+ pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+
+ pos_ee_INsensorFrame_Right = np.asarray([-0.08, 0, 0.039+0.04])
+ pos_ee_INsensorFrame_Left = np.asarray([-0.08, 0, 0.039+0.04])
+ rot_ee_INsensorFrame_Right = optas.spatialmath.roty(-np.pi/2)
+ rot_ee_INsensorFrame_Left = optas.spatialmath.roty(-np.pi/2)
+
+ self.X_ee_INsensorFrame_Right = X_fromRandP(rot_ee_INsensorFrame_Right, pos_ee_INsensorFrame_Right)
+ self.X_ee_INsensorFrame_Left = X_fromRandP(rot_ee_INsensorFrame_Left, pos_ee_INsensorFrame_Left)
+
+# self.rot_ee_right_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+# self.rot_ee_left_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+# self.pos_mass_INee_Right = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+# self.pos_mass_INee_Left = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+
+# self.mass_ee_Force = np.asarray([0,0,0,0,0,-0.3113*9.8])
+
+
+
+
+
+ # quaternion functions by given q_var_MPC
+# ori_function_R = optas.casadi.SX(np.zeros((4, self.T_MPC)))
+# ori_function_L = optas.casadi.SX(np.zeros((4, self.T_MPC)))
+# # coefficient matrix between quaternion rate and angular velocity in the global frame
+# RARM_Matrix_angular2quat = optas.casadi.SX(np.zeros((4, self.T_MPC * 3)))
+# LARM_Matrix_angular2quat = optas.casadi.SX(np.zeros((4, self.T_MPC * 3)))
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P[:, j]
+ name = 'control_point_' + str(i) + '_bound' # add position constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=lower, mid=P[:, i], rhs=upper)
+# builder_wholebodyMPC.add_equality_constraint('twoarm_miniscope' + str(i), q_var_MPC[6, i], rhs=-q_var_MPC[12, i])
+
+# ori_function_R[:, i] = ori_fnc_Right(q_var_MPC[:, i])
+# ori_function_L[:, i] = ori_fnc_Left(q_var_MPC[:, i])
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,0] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,1] = ori_function_R[:, i][2]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,2] = ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,0] = -ori_function_R[:, i][2]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,1] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,2] = ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,0] = ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,1] = -ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,2] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,0] = -ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,1] = -ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,2] = -ori_function_R[:, i][2]
+
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,0] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,1] = ori_function_L[:, i][2]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,2] = ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,0] = -ori_function_L[:, i][2]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,1] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,2] = ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,0] = ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,1] = -ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,2] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,0] = -ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,1] = -ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,2] = -ori_function_L[:, i][2]
+
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm position' + str(i), 3*optas.sumsqr(pos_fnc_Right(q_var_MPC[:, i])-pos_R[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position' + str(i), 3*optas.sumsqr(pos_fnc_Left(q_var_MPC[:, i])-pos_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), 10*optas.sumsqr(ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), 10*optas.sumsqr(ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i]+q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.05 * optas.sumsqr(P[:, i+1] - P[:, i]))
+# if(i<(self.T_MPC -1)):
+# builder_wholebodyMPC.add_cost_term('minimize_velocity2' + str(i), 0.0005 * optas.sumsqr(q_var_MPC[6:self.ndof, i+1] - q_var_MPC[6:self.ndof, i]))
+
+# obstacle_pos = np.asarray([[3.8], [0]])
+# obstacle_radius = 0.75
+# offset = np.asarray([[0.5], [0]])
+# builder_wholebodyMPC.add_geq_inequality_constraint('donkey_obstacle' + str(i), lhs=optas.norm_2(P[0:2, i] + offset - obstacle_pos), rhs=obstacle_radius**2)
+
+# if(i<(self.T_MPC -1)):
+# builder_wholebodyMPC.add_cost_term('Length' + str(i), optas.sumsqr(P[:, i+1]-P[:, i]))
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', P[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', P[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+
+# builder_wholebodyMPC.add_equality_constraint('init_velocity', lhs=(1./self.duration_MPC) * self.n * (P[0:4, 1] - P[0:4, 0]), rhs=init_velocity_MPC[0:4])
+# builder_wholebodyMPC.add_equality_constraint('init_velocity2', lhs=(1./self.duration_MPC) * self.n * (P[6:self.ndof, 1] - P[6:self.ndof, 0]), rhs=init_velocity_MPC[6:self.ndof])
+
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', P[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+
+# builder_wholebodyMPC.add_bound_inequality_constraint('init_p_equal', lhs=init_position_MPC[3:self.ndof] - np.full((15, 1), 0.005), mid=P[3:self.ndof, 0] + p_epsilon[3:self.ndof], rhs=init_position_MPC[3:self.ndof] + np.full((15, 1), 0.005))
+# builder_wholebodyMPC.add_bound_inequality_constraint('init_p_equal2', lhs=init_position_MPC[0:3] - np.full((3, 1), 0.001), mid=P[0:3, 0] + p_epsilon[0:3], rhs=init_position_MPC[0:3] + np.full((3, 1), 0.001))
+# builder_wholebodyMPC.add_cost_term('minimize_p_epsilon', optas.sumsqr(p_epsilon))
+
+# builder_wholebodyMPC.add_inequality_constraint('init_velocity', lhs=self.n * (P[0:3, 1] - P[0:3, 0]), rhs=init_velocity_MPC[0:3])
+
+
+# # get end-effector pose as parameters
+# dpos_R = builder_wholebodyMPC.add_parameter('dpos_R', 3, self.T_MPC)
+# dori_R = builder_wholebodyMPC.add_parameter('dori_R', 4, self.T_MPC)
+# dpos_L = builder_wholebodyMPC.add_parameter('dpos_L', 3, self.T_MPC)
+# dori_L = builder_wholebodyMPC.add_parameter('dori_L', 4, self.T_MPC)
+# # functions of right and left arm positions
+# dpos_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+# dpos_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+# # quaternion functions of two arm end effectors
+# dori_fnc_Right = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_right)
+# dori_fnc_Left = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_left)
+ # define dq function depending on P
+
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0001/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P[:, j+1] - P[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (P[:, i+1] - P[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_L[:, i]))
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 *self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P[:, j+2] - 2*P[:, j+1] + P[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+# # optimization cost: close to target
+# builder_wholebodyMPC.add_cost_term('Right_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_L[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm angular velocity' + str(i), optas.sumsqr(0.5 * RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3] @ dori_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dori_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm angular velocity' + str(i), optas.sumsqr(0.5 * LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3] @ dori_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i] -dori_L[:, i]))
+
+
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('ipopt', solver_options={'ipopt.print_level': 0, 'print_time': 0})
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0, 'print_time': 0,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5, 'knitro.ftol':1e-5,
+ 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1} )
+ self.ti_MPC = 0 # time index of the MPC
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ ### ---------------------------------------------------------
+ # declare ft_sensor subscriber
+ self._ft_right_sub = rospy.Subscriber("/ft_right/raw/data", WrenchStamped, self.read_ft_sensor_right_data_cb)
+ self._ft_left_sub = rospy.Subscriber("/ft_left/raw/data", WrenchStamped, self.read_ft_sensor_left_data_cb)
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber(
+# "/chonk/donkey_velocity_controller/odom",
+# Odometry,
+# self.read_base_states_cb
+# )
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseFeedback()
+ self._result = CmdChonkPoseResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([
+ acceped_goal.poseR.position.x,
+ acceped_goal.poseR.position.y,
+ acceped_goal.poseR.position.z
+ ])
+ pos_Left = np.asarray([
+ acceped_goal.poseL.position.x,
+ acceped_goal.poseL.position.y,
+ acceped_goal.poseL.position.z
+ ])
+ ori_Right = np.asarray([
+ acceped_goal.poseR.orientation.x,
+ acceped_goal.poseR.orientation.y,
+ acceped_goal.poseR.orientation.z,
+ acceped_goal.poseR.orientation.w
+ ])
+ ori_Left = np.asarray([
+ acceped_goal.poseL.orientation.x,
+ acceped_goal.poseL.orientation.y,
+ acceped_goal.poseL.orientation.z,
+ acceped_goal.poseL.orientation.w
+ ])
+ # check boundaries of the position
+ if (pos_Right > self._pos_max).any() or (pos_Right < self._pos_min).any():
+ rospy.logwarn("%s: Request aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Right[0], pos_Right[1], pos_Right[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ if (pos_Left > self._pos_max).any() or (pos_Left < self._pos_min).any():
+ rospy.logwarn("%s: Lequest aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Left[0], pos_Left[1], pos_Left[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name,
+ pos_Right[0], pos_Right[1], pos_Right[2],
+ ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2],
+ ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3],
+ acceped_goal.duration
+ )
+ )
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### optas
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+
+# print(np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=np.zeros(18))).T[0])
+# print(np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=np.zeros(18))).T[0])
+# print(np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=np.zeros(18))).T[0])
+# print(np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=np.zeros(18))).T[0])
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - start_RARM_pos
+ Derivation_RARM_Quat = ori_Right - start_RARM_quat
+ Derivation_LARM_Pos = pos_Left - start_LARM_pos
+ Derivation_LARM_Quat = ori_Left - start_LARM_quat
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._DRARM_ee_Pos_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_RARM_Pos/self.duration)
+ self._LARM_ee_Pos_trajectory = lambda t: start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._DLARM_ee_Pos_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_LARM_Pos/self.duration)
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._DRARM_ee_Quat_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_RARM_Quat/self.duration)
+ self._LARM_ee_Quat_trajectory = lambda t: start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ self._DLARM_ee_Quat_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_LARM_Quat/self.duration)
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+
+ """Sensor data in ee frame"""
+ ft_ee_right = self.X_ee_INsensorFrame_Right.T @ self.ft_right
+ ft_ee_left = self.X_ee_INsensorFrame_Left.T @ self.ft_left
+# """ee mass data in ee frame"""
+# ft_ee_mass_right = X_fromRandP_different(self.rot_ee_right_fnc_global(self.q_curr), self.pos_mass_INee_Right).T @ self.mass_ee_Force
+# ft_ee_mass_left = X_fromRandP_different(self.rot_ee_left_fnc_global(self.q_curr), self.pos_mass_INee_Left).T @ self.mass_ee_Force
+# """Sensor data delete mass influence"""
+# ft_ee_composite_right = ft_ee_right + ft_ee_mass_right
+# ft_ee_composite_left = ft_ee_left + ft_ee_mass_left
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+
+ pos_R_goal = []
+ ori_R_goal = []
+ pos_L_goal = []
+ ori_L_goal = []
+ dpos_R_goal = []
+ dori_R_goal = []
+ dpos_L_goal = []
+ dori_L_goal = []
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+# print(g_rarm_ee_ori[0]**2+g_rarm_ee_ori[1]**2+g_rarm_ee_ori[2]**2 +g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+
+# dg_rarm_ee_pos = self._DRARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# dpos_R_goal.append(dg_rarm_ee_pos.tolist())
+# dg_rarm_ee_ori = self._DRARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# dori_R_goal.append(dg_rarm_ee_ori.tolist())
+# dg_larm_ee_pos = self._DLARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# dpos_L_goal.append(dg_larm_ee_pos.tolist())
+# dg_larm_ee_ori = self._DLARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# dori_L_goal.append(dg_larm_ee_ori.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+# dpos_R_goal.append(dg_rarm_ee_pos.tolist()) # i.e. previous goal
+# dori_R_goal.append(dg_rarm_ee_ori.tolist()) # i.e. previous goal
+# dpos_L_goal.append(dg_rarm_ee_pos.tolist()) # i.e. previous goal
+# dori_L_goal.append(dg_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+# dpos_R_goal = optas.np.array(dpos_R_goal).T
+# dori_R_goal = optas.np.array(dori_R_goal).T
+# dpos_L_goal = optas.np.array(dpos_L_goal).T
+# dori_L_goal = optas.np.array(dori_L_goal).T
+
+
+
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'P': self.curr_MPC})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'P': self.solution_MPC[f'P']})
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal, 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr } )
+# self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal, 't': self.timebyT} )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ P = np.asarray(self.solution_MPC[f'P'])
+
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC
+ n = self.T_MPC -1
+ q_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * P[:, j]
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P[:, j+1] - P[:, j])
+# q_next = P[:, 0]
+# dq_next = self.n * (P[:, 1] - P[:, 0])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = q_next[-self.ndof_position_control:].tolist()
+# self._msg.data = q_next[-self.ndof_position_control:]
+# self._msg.data = [float('%.3f' % x) for x in self._msg.data]
+# print(self._msg.data)
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_ft_sensor_right_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_right = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_ft_sensor_left_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_left = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+# base_linear_velocity_global = self.donkey_R @ self.donkey_velocity
+# base_angular_velocity_global = self.donkey_R @ self.donkey_angular_velocity
+# self.dq_curr_base = [base_linear_velocity_global[0], base_linear_velocity_global[1], base_angular_velocity_global[2]]
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_obstable_sensorAD_notGood.py b/script/action_server_cmd_pose_MPC_BC_operational_obstable_sensorAD_notGood.py
new file mode 100755
index 0000000..b63be93
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_obstable_sensorAD_notGood.py
@@ -0,0 +1,725 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq',20)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+ self.dt_MPC_planner = 0.1 # time step
+ self.T_MPC_planner = 8 # T is number of control points of expected r_arm_right and r_arm_left
+ self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+# self.N_f = 8 # This is the number of control points of expected contact force of right and left arms
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_Right = builder_wholebodyMPC_planner.add_decision_variables('R_Right', 3, self.T_MPC_planner)
+ R_Left = builder_wholebodyMPC_planner.add_decision_variables('R_Left', 3, self.T_MPC_planner)
+ R_middle = builder_wholebodyMPC_planner.add_decision_variables('R_middle', 3, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+
+ t = builder_wholebodyMPC_planner.add_parameter('t', self.T_MPC_planner) # time
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+ # Add parameters
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 3)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 3)
+ init_dr_velocity_Right = builder_wholebodyMPC_planner.add_parameter('init_dr_velocity_Right', 3)
+ init_dr_velocity_Left = builder_wholebodyMPC_planner.add_parameter('init_dr_velocity_Left', 3)
+ sensor_Right = builder_wholebodyMPC_planner.add_parameter('sensor_Right', 3)
+ sensor_Left = builder_wholebodyMPC_planner.add_parameter('sensor_Left', 3)
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3, self.T_MPC_planner)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3, self.T_MPC_planner)
+ F_Right_target = builder_wholebodyMPC_planner.add_parameter('F_Right_target', 3, self.T_MPC_planner)
+ F_Left_target = builder_wholebodyMPC_planner.add_parameter('F_Left_target', 3, self.T_MPC_planner)
+
+ # define position variables of middle and two-ee points
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+ r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Right[:, j]
+ r_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Left[:, j]
+ builder_wholebodyMPC_planner.add_cost_term('r_middle_x' + str(i), optas.sumsqr(r_middle_var_MPC[0, i] - 0.5*(pos_R[0, i] + pos_L[0, i])))
+ builder_wholebodyMPC_planner.add_cost_term('r_middle_y' + str(i), 10*optas.sumsqr(r_middle_var_MPC[1, i] - 0.5*(pos_R[1, i] + pos_L[1, i])))
+# builder_wholebodyMPC_planner.add_cost_term('r_middle_z' + str(i), optas.sumsqr(r_middle_var_MPC[2, i] - 0.5*(pos_R[2, :] + pos_L[2, i])))
+ builder_wholebodyMPC_planner.add_equality_constraint('r_middle_z', r_middle_var_MPC[2, :], rhs=0.5*(pos_R[2, :]+pos_L[2, :]))
+
+ # add position constraint at the beginning state
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[0:2, 0], rhs=0.5*(init_r_position_Right[0:2] + init_r_position_Left[0:2]))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_right', R_Right[:, 0], rhs=init_r_position_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_left', R_Left[:, 0], rhs=init_r_position_Left)
+
+ for i in range(self.T_MPC_planner):
+ # define the cost functions between the middle point and the two arm end effectors
+ builder_wholebodyMPC_planner.add_cost_term('Right_arm_xy'+str(i), optas.sumsqr(r_RARM_var_MPC[0:2, i] - (r_middle_var_MPC[0:2, i] - 0.5*(pos_L[0:2, i]-pos_R[0:2, i]))))
+ builder_wholebodyMPC_planner.add_cost_term('Left_arm_xy'+str(i), optas.sumsqr(r_LARM_var_MPC[0:2, i] - (r_middle_var_MPC[0:2, i] + 0.5*(pos_L[0:2, i]-pos_R[0:2, i]))))
+ builder_wholebodyMPC_planner.add_cost_term('Right_arm_z'+str(i), optas.sumsqr(r_RARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_arm_z'+str(i), optas.sumsqr(r_LARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+
+ # obstacle avoidance by using the distance between the r_middle point and the obstacle center
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', optas.sumsqr(r_ep))
+
+ # define the velocity variables of the middle and two-ee points
+ dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ w_dr = 0.0001/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+ dr_middle_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+ dr_RARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Right[:, j+1] - R_Right[:, j])
+ dr_LARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Left[:, j+1] - R_Left[:, j])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_right' + str(i), w_dr * optas.sumsqr(dr_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_left' + str(i), w_dr * optas.sumsqr(dr_LARM_var_MPC[:, i]))
+
+ # define the acceleration variables of the middle and two-ee points
+ ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ w_ddr = 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+ ddr_middle_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+ ddr_RARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Right[:, j+2] - 2*R_Right[:, j+1] + R_Right[:, j])
+ ddr_LARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Left[:, j+2] - 2*R_Left[:, j+1] + R_Left[:, j])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_right' + str(i), w_ddr * optas.sumsqr(ddr_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_left' + str(i), w_ddr * optas.sumsqr(ddr_LARM_var_MPC[:, i]))
+
+ # define two-ee functions about positions and velocities
+ self.r_RARM_position_fun = pos_fnc_Right = self.wholebodyMPC_planner.get_global_link_position_function(link=self._link_ee_right)
+ self.r_LARM_position_fun = self.wholebodyMPC_planner.get_global_link_position_function(link=self._link_ee_left)
+ self.dr_RARM_Jacobian_fun = self.wholebodyMPC_planner.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.dr_LARM_Jacobian_fun = self.wholebodyMPC_planner.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+
+ # define two-arm ee contact force variables
+ F_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ F_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ self.K_Right = np.diag([1000, 1000, 1000]) # Stiffness Right
+ self.K_Left = np.diag([1000, 1000, 1000]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(2*self.K_Right[0,0]), 2 * np.sqrt(2*self.K_Right[1,1]), 2 * np.sqrt(2*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(2*self.K_Left[0,0]), 2 * np.sqrt(2*self.K_Left[1,1]), 2 * np.sqrt(2*self.K_Left[2,2])]) # Damping Left
+ for i in range(self.T_MPC_planner):
+ F_RARM_var_MPC[:, i] = sensor_Right - self.K_Right @ (r_RARM_var_MPC[:, i] - init_r_position_Right) - self.D_Right @ (dr_RARM_var_MPC[:, i] - init_dr_velocity_Right)
+ F_LARM_var_MPC[:, i] = sensor_Left - self.K_Left @ (r_LARM_var_MPC[:, i] - init_r_position_Left) - self.D_Left @ (dr_LARM_var_MPC[:, i] - init_dr_velocity_Left)
+ builder_wholebodyMPC_planner.add_cost_term('Right_arm Force' + str(i), optas.sumsqr(F_RARM_var_MPC[1, i] - F_Right_target[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_arm Force' + str(i), optas.sumsqr(F_LARM_var_MPC[1, i] - F_Left_target[1, i]))
+
+ pos_ee_INsensorFrame_Right = np.asarray([-0.08, 0, 0.039+0.04])
+ pos_ee_INsensorFrame_Left = np.asarray([-0.08, 0, 0.039+0.04])
+ rot_ee_INsensorFrame_Right = optas.spatialmath.roty(-np.pi/2)
+ rot_ee_INsensorFrame_Left = optas.spatialmath.roty(-np.pi/2)
+
+ self.X_ee_INsensorFrame_Right = X_fromRandP(rot_ee_INsensorFrame_Right, pos_ee_INsensorFrame_Right)
+ self.X_ee_INsensorFrame_Left = X_fromRandP(rot_ee_INsensorFrame_Left, pos_ee_INsensorFrame_Left)
+
+ self.rot_ee_right_fnc_global = self.wholebodyMPC_planner.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rot_ee_left_fnc_global = self.wholebodyMPC_planner.get_global_link_rotation_function(link=self._link_ee_left)
+ self.pos_mass_INee_Right = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+ self.pos_mass_INee_Left = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+
+ self.mass_ee_Force = np.asarray([0,0,0,0,0,-0.3113*9.8])
+
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0, 'print_time': 0,
+ 'knitro.FeasTol': 1e-6, 'knitro.OptTol': 1e-6, 'knitro.ftol':1e-6,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+ self.time_linspace_planner = np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner)
+ self.timebyT_planner = np.asarray(self.time_linspace_planner)/self.duration_MPC_planner
+
+ self.start_RARM_force = np.zeros(3)
+# self.start_RARM_torque = np.zeros(3)
+ self.start_LARM_force = np.zeros(3)
+# self.start_LARM_torque = np.zeros(3)
+ ### -------------------------------------------------------------------------------------------------------------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyMPC_LIMITS'
+ )
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 8 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+
+
+ # get end-effector pose as parameters
+ pos_R_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ pos_L_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ R_Right_reasonal = builder_wholebodyMPC.add_parameter('R_Right_reasonal', 3, self.T_MPC_planner)
+ R_Left_reasonal = builder_wholebodyMPC.add_parameter('R_Left_reasonal', 3, self.T_MPC_planner)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+
+ # functions of right and left arm positions
+ pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+
+ fac = self.duration_MPC/self.duration_MPC_planner
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t[i]*fac)**j * (1-t[i]*fac)**(self.n_planner-j) * R_Right_reasonal[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t[i]*fac)**j * (1-t[i]*fac)**(self.n_planner-j) * R_Left_reasonal[:, j]
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+
+
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm position' + str(i), optas.sumsqr(pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] ))
+ builder_wholebodyMPC.add_cost_term('Left_arm position' + str(i), optas.sumsqr(pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] ))
+
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.01 * optas.sumsqr(q_var_MPC[6, i]+q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('head_miniscope' + str(i), 0.01 * optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.002 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.001 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0001/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_deriv_' + str(i) + '_bound', lhs=dlower, mid=self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity_dq' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+
+ # setup solver
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0, 'print_time': 0,
+ 'knitro.FeasTol': 1e-6, 'knitro.OptTol': 1e-6, 'knitro.ftol':1e-6,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+ ### ---------------------------------------------------------------------------------------------------------------------------------------------------
+ # declare ft_sensor subscriber
+ self._ft_right_sub = rospy.Subscriber("/ft_right/raw/data", WrenchStamped, self.read_ft_sensor_right_data_cb)
+ self._ft_left_sub = rospy.Subscriber("/ft_left/raw/data", WrenchStamped, self.read_ft_sensor_left_data_cb)
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/streaming_controller/command", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp inertia publisher
+ self._inertia_sub = rospy.Subscriber("/chonk/arm_ee_inertias", Float64MultiArray, self.read_two_ee_grasp_inertia_data_cb)
+ self._inertia_1DOF_sub = rospy.Subscriber("/chonk/arm_ee_1DOF_inertias_1DOF", Float64MultiArray, self.read_two_ee_grasp_inertia_1DOF_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+# torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+# torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, pos_Right[0], pos_Right[1], pos_Right[2], ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2], ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - self.start_RARM_pos
+ Derivation_RARM_Quat = ori_Right - self.start_RARM_quat
+ Derivation_LARM_Pos = pos_Left - self.start_LARM_pos
+ Derivation_LARM_Quat = ori_Left - self.start_LARM_quat
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force
+# Derivation_RARM_torque = torque_Right - self.start_RARM_torque
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force
+# Derivation_LARM_torque = torque_Left - self.start_LARM_torque
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+# self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+# self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+
+ # increment idx (in here counts starts with 1)
+ self.ti_MPC_planner = 0 # time index of the MPC
+ self._idx += 1
+ pos_R_goal = []
+ pos_L_goal = []
+ force_R_goal = []
+ force_L_goal = []
+ for i in range(self.T_MPC_planner):
+ if(self.ti_MPC_planner <= self.duration):
+ self.ti_MPC_planner = self._t[self._idx-1] + self.dt_MPC_planner*i
+ if(self.ti_MPC_planner > self.duration):
+ self.ti_MPC_planner = self.duration
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC_planner).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC_planner).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC_planner).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC_planner).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_actual_Right = np.array(self.r_RARM_position_fun(self.q_curr))
+ r_actual_Left = np.array(self.r_LARM_position_fun(self.q_curr))
+
+ dr_actual_Right = np.asarray(self.dr_RARM_Jacobian_fun(self.q_curr)) @ self.dq_curr
+ dr_actual_Left = np.asarray(self.dr_LARM_Jacobian_fun(self.q_curr)) @ self.dq_curr
+
+ """Sensor data in global frame"""
+ global_X_ee_Right = X_fromRandP(np.asarray(self.rot_ee_right_fnc_global(self.q_curr)), np.zeros(3))
+ global_X_ee_Left = X_fromRandP(np.asarray(self.rot_ee_left_fnc_global(self.q_curr)), np.zeros(3))
+
+ ft_ee_right = np.linalg.inv(global_X_ee_Right).T @ self.X_ee_INsensorFrame_Right.T @ self.ft_right
+ ft_ee_left = np.linalg.inv(global_X_ee_Left).T @ self.X_ee_INsensorFrame_Left.T @ self.ft_left
+
+ print("Right force in global frame:")
+ print(ft_ee_right[3:6])
+ print("Left force in global frame:")
+ print(ft_ee_left[3:6])
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_middle': 0.5*(pos_R_goal+pos_L_goal),
+ f'R_Right': pos_R_goal,
+ f'R_Left': pos_L_goal,
+ f'r_ep': np.zeros(self.T_MPC_planner) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_middle': self.solution_MPC_planner[f'R_middle'],
+ f'R_Right': self.solution_MPC_planner[f'R_Right'],
+ f'R_Left': self.solution_MPC_planner[f'R_Left'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'] })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': pos_R_goal, 'pos_L': pos_L_goal,
+ 't': self.timebyT_planner,
+ 'init_r_position_Right': r_actual_Right,
+ 'init_r_position_Left': r_actual_Left,
+ 'init_dr_velocity_Right': dr_actual_Right,
+ 'init_dr_velocity_Left': dr_actual_Left,
+ 'sensor_Right': ft_ee_right[3:6], 'sensor_Left': ft_ee_left[3:6],
+ 'F_Right_target': force_R_goal,
+ 'F_Left_target': force_L_goal} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_Right = np.asarray(self.solution_MPC_planner[f'R_Right'])
+ R_Left = np.asarray(self.solution_MPC_planner[f'R_Left'])
+ ### -----------------------------------------------------------
+ self.ti_MPC = 0
+ ori_R_goal = []
+ ori_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC_planner).flatten()
+ g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC_planner).flatten()
+ g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ ### optas. Solve the whole-body MPC
+ ### ---------------------------------------------------------
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC })
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({ f'Q': self.solution_MPC[f'Q']})
+
+ self.solver_wholebodyMPC.reset_parameters({'ori_R_reasonal': ori_R_goal, 'ori_L_reasonal': ori_L_goal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr,
+ 'init_velocity_MPC': self.dq_curr,
+ 'R_Right_reasonal': R_Right, 'R_Left_reasonal': R_Left } )
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC
+ n = self.T_MPC -1
+ q_next = np.zeros(self.ndof)
+ p_right = np.zeros(3)
+ dp_right = np.zeros(3)
+ ddp_right = np.zeros(3)
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_ft_sensor_right_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_right = np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_ft_sensor_left_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_left = np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+ def read_two_ee_grasp_inertia_data_cb(self, msg):
+ self.Lambda_ee_Right = np.asarray([ [msg.data[0], msg.data[1], msg.data[2]],
+ [msg.data[1], msg.data[3], msg.data[4]],
+ [msg.data[2], msg.data[4], msg.data[5]] ])
+ self.Lambda_ee_Left = np.asarray([ [msg.data[6], msg.data[7], msg.data[8]],
+ [msg.data[7], msg.data[9], msg.data[10]],
+ [msg.data[8], msg.data[10], msg.data[11]] ])
+
+
+ def read_two_ee_grasp_inertia_1DOF_data_cb(self, msg):
+ self.Lambda_ee_1DOF_Right = np.asarray([ msg.data[0] ])
+ self.Lambda_ee_1DOF_Left = np.asarray([ msg.data[1] ])
+ self.Lambda_ee_Right[1, 1] = msg.data[0]
+ self.Lambda_ee_Left[1, 1] = msg.data[1]
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_experiment.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_experiment.py
new file mode 100755
index 0000000..92c7c40
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_experiment.py
@@ -0,0 +1,718 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseFeedback, CmdChonkPoseResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+# self._link_sensor_right = rospy.get_param('~link_sensor_right', 'link_sensor_right')
+# self._link_sensor_left = rospy.get_param('~link_sensor_left', 'link_sensor_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 20)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ ### optas
+ self.tf_listener = tf.TransformListener()
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyMPC_LIMITS'
+ )
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 6 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ P = builder_wholebodyMPC.add_decision_variables('P', self.ndof, self.T_MPC)
+# p_epsilon = builder_wholebodyMPC.add_decision_variables('p_epsilon', self.ndof)
+
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+ ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+ pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+ ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+ # functions of right and left arm positions
+ pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+
+ pos_ee_INsensorFrame_Right = np.asarray([-0.08, 0, 0.039+0.04])
+ pos_ee_INsensorFrame_Left = np.asarray([-0.08, 0, 0.039+0.04])
+ rot_ee_INsensorFrame_Right = optas.spatialmath.roty(-np.pi/2)
+ rot_ee_INsensorFrame_Left = optas.spatialmath.roty(-np.pi/2)
+
+ self.X_ee_INsensorFrame_Right = X_fromRandP(rot_ee_INsensorFrame_Right, pos_ee_INsensorFrame_Right)
+ self.X_ee_INsensorFrame_Left = X_fromRandP(rot_ee_INsensorFrame_Left, pos_ee_INsensorFrame_Left)
+
+# self.rot_ee_right_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+# self.rot_ee_left_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+# self.pos_mass_INee_Right = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+# self.pos_mass_INee_Left = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+
+# self.mass_ee_Force = np.asarray([0,0,0,0,0,-0.3113*9.8])
+
+
+
+
+
+ # quaternion functions by given q_var_MPC
+# ori_function_R = optas.casadi.SX(np.zeros((4, self.T_MPC)))
+# ori_function_L = optas.casadi.SX(np.zeros((4, self.T_MPC)))
+# # coefficient matrix between quaternion rate and angular velocity in the global frame
+# RARM_Matrix_angular2quat = optas.casadi.SX(np.zeros((4, self.T_MPC * 3)))
+# LARM_Matrix_angular2quat = optas.casadi.SX(np.zeros((4, self.T_MPC * 3)))
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P[:, j]
+ name = 'control_point_' + str(i) + '_bound' # add position constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=lower, mid=P[:, i], rhs=upper)
+# builder_wholebodyMPC.add_equality_constraint('twoarm_miniscope' + str(i), q_var_MPC[6, i], rhs=-q_var_MPC[12, i])
+
+# ori_function_R[:, i] = ori_fnc_Right(q_var_MPC[:, i])
+# ori_function_L[:, i] = ori_fnc_Left(q_var_MPC[:, i])
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,0] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,1] = ori_function_R[:, i][2]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,2] = ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,0] = -ori_function_R[:, i][2]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,1] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,2] = ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,0] = ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,1] = -ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,2] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,0] = -ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,1] = -ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,2] = -ori_function_R[:, i][2]
+
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,0] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,1] = ori_function_L[:, i][2]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,2] = ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,0] = -ori_function_L[:, i][2]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,1] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,2] = ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,0] = ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,1] = -ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,2] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,0] = -ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,1] = -ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,2] = -ori_function_L[:, i][2]
+
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm position' + str(i), optas.sumsqr(pos_fnc_Right(q_var_MPC[:, i])-pos_R[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position' + str(i), optas.sumsqr(pos_fnc_Left(q_var_MPC[:, i])-pos_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i]+q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.001 * optas.sumsqr(P[:, i+1] - P[:, i]))
+# if(i<(self.T_MPC -1)):
+# builder_wholebodyMPC.add_cost_term('minimize_velocity2' + str(i), 0.0005 * optas.sumsqr(q_var_MPC[6:self.ndof, i+1] - q_var_MPC[6:self.ndof, i]))
+
+# obstacle_pos = np.asarray([[3.8], [0]])
+# obstacle_radius = 0.75
+# offset = np.asarray([[0.5], [0]])
+# builder_wholebodyMPC.add_geq_inequality_constraint('donkey_obstacle' + str(i), lhs=optas.norm_2(P[0:2, i] + offset - obstacle_pos), rhs=obstacle_radius**2)
+
+# if(i<(self.T_MPC -1)):
+# builder_wholebodyMPC.add_cost_term('Length' + str(i), optas.sumsqr(P[:, i+1]-P[:, i]))
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', P[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', P[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', P[4, :], rhs=np.full((1, self.T_MPC), 0.0 ) )
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope1', P[5, :], rhs=np.full((1, self.T_MPC), -0.25) )
+
+
+# builder_wholebodyMPC.add_bound_inequality_constraint('init_p_equal', lhs=init_position_MPC[3:self.ndof] - np.full((15, 1), 0.005), mid=P[3:self.ndof, 0] + p_epsilon[3:self.ndof], rhs=init_position_MPC[3:self.ndof] + np.full((15, 1), 0.005))
+# builder_wholebodyMPC.add_bound_inequality_constraint('init_p_equal2', lhs=init_position_MPC[0:3] - np.full((3, 1), 0.001), mid=P[0:3, 0] + p_epsilon[0:3], rhs=init_position_MPC[0:3] + np.full((3, 1), 0.001))
+# builder_wholebodyMPC.add_cost_term('minimize_p_epsilon', optas.sumsqr(p_epsilon))
+
+# builder_wholebodyMPC.add_inequality_constraint('init_velocity', lhs=self.n * (P[0:3, 1] - P[0:3, 0]), rhs=init_velocity_MPC[0:3])
+
+
+# # get end-effector pose as parameters
+# dpos_R = builder_wholebodyMPC.add_parameter('dpos_R', 3, self.T_MPC)
+# dori_R = builder_wholebodyMPC.add_parameter('dori_R', 4, self.T_MPC)
+# dpos_L = builder_wholebodyMPC.add_parameter('dpos_L', 3, self.T_MPC)
+# dori_L = builder_wholebodyMPC.add_parameter('dori_L', 4, self.T_MPC)
+# # functions of right and left arm positions
+# dpos_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+# dpos_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+# # quaternion functions of two arm end effectors
+# dori_fnc_Right = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_right)
+# dori_fnc_Left = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_left)
+ # define dq function depending on P
+
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0001/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P[:, j+1] - P[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=self.n * (P[:, i+1] - P[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_L[:, i]))
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P[:, j+2] - 2*P[:, j+1] + P[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+# # optimization cost: close to target
+# builder_wholebodyMPC.add_cost_term('Right_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_L[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm angular velocity' + str(i), optas.sumsqr(0.5 * RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3] @ dori_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dori_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm angular velocity' + str(i), optas.sumsqr(0.5 * LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3] @ dori_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i] -dori_L[:, i]))
+
+
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('ipopt', solver_options={'ipopt.print_level': 0, 'print_time': 0})
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0, 'print_time': 0,
+ 'knitro.FeasTol': 1e-6, 'knitro.OptTol': 1e-6, 'knitro.ftol':1e-6,
+ 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1} )
+ self.ti_MPC = 0 # time index of the MPC
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ ### ---------------------------------------------------------
+ # declare ft_sensor subscriber
+# self._ft_right_sub = rospy.Subscriber(
+# "/ft_right/raw/data",
+# WrenchStamped,
+# self.read_ft_sensor_right_data_cb
+# )
+# self._ft_left_sub = rospy.Subscriber(
+# "/ft_left/raw/data",
+# WrenchStamped,
+# self.read_ft_sensor_left_data_cb
+# )
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/chonk/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+# self._joint_sub_base = rospy.Subscriber(
+# "/chonk/donkey_velocity_controller/odom",
+# Odometry,
+# self.read_base_states_cb
+# )
+# self._joint_sub_base = rospy.Subscriber(
+# "/tf",
+# Odometry,
+# self.read_base_states_cb
+# )
+# self._joint_sub_base = rospy.Subscriber(
+# "/chonk/base_pose_ground_truth",
+# Odometry,
+# self.read_base_states_cb
+# )
+ # declare joint publisher
+# self._joint_pub = rospy.Publisher(
+# self._pub_cmd_topic_name,
+# Float64MultiArray,
+# queue_size=10
+# )
+# self._joint_pub = rospy.Publisher(
+# "/chonk/streaming_controller/command",
+# Float64MultiArray,
+# queue_size=10
+# )
+ self._joint_pub = rospy.Publisher(
+ "/chonk/trajectory_controller/command",
+ JointTrajectory,
+ queue_size=10
+ )
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher(
+ "/chonk/donkey_velocity_controller/cmd_vel",
+ Twist,
+ queue_size=10
+ )
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ # initialize action messages
+ self._feedback = CmdChonkPoseFeedback()
+ self._result = CmdChonkPoseResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'cmd_pose',
+ CmdChonkPoseAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([
+ acceped_goal.poseR.position.x,
+ acceped_goal.poseR.position.y,
+ acceped_goal.poseR.position.z
+ ])
+ pos_Left = np.asarray([
+ acceped_goal.poseL.position.x,
+ acceped_goal.poseL.position.y,
+ acceped_goal.poseL.position.z
+ ])
+ ori_Right = np.asarray([
+ acceped_goal.poseR.orientation.x,
+ acceped_goal.poseR.orientation.y,
+ acceped_goal.poseR.orientation.z,
+ acceped_goal.poseR.orientation.w
+ ])
+ ori_Left = np.asarray([
+ acceped_goal.poseL.orientation.x,
+ acceped_goal.poseL.orientation.y,
+ acceped_goal.poseL.orientation.z,
+ acceped_goal.poseL.orientation.w
+ ])
+ # check boundaries of the position
+ if (pos_Right > self._pos_max).any() or (pos_Right < self._pos_min).any():
+ rospy.logwarn("%s: Request aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Right[0], pos_Right[1], pos_Right[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ if (pos_Left > self._pos_max).any() or (pos_Left < self._pos_min).any():
+ rospy.logwarn("%s: Lequest aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Left[0], pos_Left[1], pos_Left[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name,
+ pos_Right[0], pos_Right[1], pos_Right[2],
+ ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2],
+ ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3],
+ acceped_goal.duration
+ )
+ )
+ # read current robot joint positions
+ try:
+ (trans,rot) = self.tf_listener.lookupTransform('/vicon/world', 'vicon/chonk/CHONK', rospy.Time(0))
+ except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+ print("error: cannot find vicon data!!!!")
+ self.base_euler_angle = tf.transformations.euler_from_quaternion([rot[0], rot[1], rot[2], rot[3]])
+ self.q_curr_base = [trans[0], trans[1], self.base_euler_angle[2]]
+
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### optas
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - start_RARM_pos
+ Derivation_RARM_Quat = ori_Right - start_RARM_quat
+ Derivation_LARM_Pos = pos_Left - start_LARM_pos
+ Derivation_LARM_Quat = ori_Left - start_LARM_quat
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._DRARM_ee_Pos_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_RARM_Pos/self.duration)
+ self._LARM_ee_Pos_trajectory = lambda t: start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._DLARM_ee_Pos_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_LARM_Pos/self.duration)
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._DRARM_ee_Quat_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_RARM_Quat/self.duration)
+ self._LARM_ee_Quat_trajectory = lambda t: start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ self._DLARM_ee_Quat_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_LARM_Quat/self.duration)
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+ try:
+ (trans,rot) = self.tf_listener.lookupTransform('/vicon/world', 'vicon/chonk/CHONK', rospy.Time(0))
+ except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+ print("error: cannot find vicon data!!!!")
+ self.base_euler_angle = tf.transformations.euler_from_quaternion([rot[0], rot[1], rot[2], rot[3]])
+ self.q_curr_base = [trans[0], trans[1], self.base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(self.base_euler_angle[2])
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+# """ee mass data in ee frame"""
+# ft_ee_mass_right = X_fromRandP_different(self.rot_ee_right_fnc_global(self.q_curr), self.pos_mass_INee_Right).T @ self.mass_ee_Force
+# ft_ee_mass_left = X_fromRandP_different(self.rot_ee_left_fnc_global(self.q_curr), self.pos_mass_INee_Left).T @ self.mass_ee_Force
+# """Sensor data delete mass influence"""
+# ft_ee_composite_right = ft_ee_right + ft_ee_mass_right
+# ft_ee_composite_left = ft_ee_left + ft_ee_mass_left
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+
+ pos_R_goal = []
+ ori_R_goal = []
+ pos_L_goal = []
+ ori_L_goal = []
+ dpos_R_goal = []
+ dori_R_goal = []
+ dpos_L_goal = []
+ dori_L_goal = []
+
+ for i in range(self.T_MPC):
+# self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+# print(g_rarm_ee_ori[0]**2+g_rarm_ee_ori[1]**2+g_rarm_ee_ori[2]**2 +g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+
+# dg_rarm_ee_pos = self._DRARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# dpos_R_goal.append(dg_rarm_ee_pos.tolist())
+# dg_rarm_ee_ori = self._DRARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# dori_R_goal.append(dg_rarm_ee_ori.tolist())
+# dg_larm_ee_pos = self._DLARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# dpos_L_goal.append(dg_larm_ee_pos.tolist())
+# dg_larm_ee_ori = self._DLARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# dori_L_goal.append(dg_larm_ee_ori.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+# dpos_R_goal.append(dg_rarm_ee_pos.tolist()) # i.e. previous goal
+# dori_R_goal.append(dg_rarm_ee_ori.tolist()) # i.e. previous goal
+# dpos_L_goal.append(dg_rarm_ee_pos.tolist()) # i.e. previous goal
+# dori_L_goal.append(dg_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+# dpos_R_goal = optas.np.array(dpos_R_goal).T
+# dori_R_goal = optas.np.array(dori_R_goal).T
+# dpos_L_goal = optas.np.array(dpos_L_goal).T
+# dori_L_goal = optas.np.array(dori_L_goal).T
+
+
+
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'P': self.curr_MPC})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'P': self.solution_MPC[f'P']})
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal, 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr } )
+# self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal, 't': self.timebyT} )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ P = np.asarray(self.solution_MPC[f'P'])
+
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC
+ n = self.T_MPC -1
+ q_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * P[:, j]
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P[:, j+1] - P[:, j])
+# q_next = P[:, 0]
+# dq_next = self.n * (P[:, 1] - P[:, 0])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+# self._msg.data[0:12] = q_next[-12:]
+# self._msg.data[12:15] = q_next[3:6]
+# self.eva_point.positions = q_next[-self.ndof_position_control:].tolist()
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = q_next[-self.ndof_position_control:].tolist()
+# print(q_next[-self.ndof_position_control:])
+
+# self._msg.data = [float('%.3f' % x) for x in self._msg.data]
+# self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2] * 6.05
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_ft_sensor_right_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_right = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_ft_sensor_left_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_left = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+# def read_base_states_cb(self, msg):
+# base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+# self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+# self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+# self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+# self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+# self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+# self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_experiment_bigbox.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_experiment_bigbox.py
new file mode 100755
index 0000000..013e43b
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_experiment_bigbox.py
@@ -0,0 +1,733 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseFeedback, CmdChonkPoseResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+# self._link_sensor_right = rospy.get_param('~link_sensor_right', 'link_sensor_right')
+# self._link_sensor_left = rospy.get_param('~link_sensor_left', 'link_sensor_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 20)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ ### optas
+ self.tf_listener = tf.TransformListener()
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyMPC_LIMITS'
+ )
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 6 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ P = builder_wholebodyMPC.add_decision_variables('P', self.ndof, self.T_MPC)
+# p_epsilon = builder_wholebodyMPC.add_decision_variables('p_epsilon', self.ndof)
+
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+ ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+ pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+ ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+ # functions of right and left arm positions
+ pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+
+ pos_ee_INsensorFrame_Right = np.asarray([-0.08, 0, 0.039+0.04])
+ pos_ee_INsensorFrame_Left = np.asarray([-0.08, 0, 0.039+0.04])
+ rot_ee_INsensorFrame_Right = optas.spatialmath.roty(-np.pi/2)
+ rot_ee_INsensorFrame_Left = optas.spatialmath.roty(-np.pi/2)
+
+ self.X_ee_INsensorFrame_Right = X_fromRandP(rot_ee_INsensorFrame_Right, pos_ee_INsensorFrame_Right)
+ self.X_ee_INsensorFrame_Left = X_fromRandP(rot_ee_INsensorFrame_Left, pos_ee_INsensorFrame_Left)
+
+# self.rot_ee_right_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+# self.rot_ee_left_fnc_global = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+# self.pos_mass_INee_Right = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+# self.pos_mass_INee_Left = np.asarray([-0.039-0.04+0.047729, 0, -0.08+0.017732])
+
+# self.mass_ee_Force = np.asarray([0,0,0,0,0,-0.3113*9.8])
+
+
+
+
+
+ # quaternion functions by given q_var_MPC
+# ori_function_R = optas.casadi.SX(np.zeros((4, self.T_MPC)))
+# ori_function_L = optas.casadi.SX(np.zeros((4, self.T_MPC)))
+# # coefficient matrix between quaternion rate and angular velocity in the global frame
+# RARM_Matrix_angular2quat = optas.casadi.SX(np.zeros((4, self.T_MPC * 3)))
+# LARM_Matrix_angular2quat = optas.casadi.SX(np.zeros((4, self.T_MPC * 3)))
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P[:, j]
+ name = 'control_point_' + str(i) + '_bound' # add position constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=lower, mid=P[:, i], rhs=upper)
+# builder_wholebodyMPC.add_equality_constraint('twoarm_miniscope' + str(i), q_var_MPC[6, i], rhs=-q_var_MPC[12, i])
+
+# ori_function_R[:, i] = ori_fnc_Right(q_var_MPC[:, i])
+# ori_function_L[:, i] = ori_fnc_Left(q_var_MPC[:, i])
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,0] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,1] = ori_function_R[:, i][2]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,2] = ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,0] = -ori_function_R[:, i][2]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,1] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,2] = ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,0] = ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,1] = -ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,2] = ori_function_R[:, i][3]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,0] = -ori_function_R[:, i][0]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,1] = -ori_function_R[:, i][1]
+# RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,2] = -ori_function_R[:, i][2]
+
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,0] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,1] = ori_function_L[:, i][2]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][0,2] = ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,0] = -ori_function_L[:, i][2]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,1] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][1,2] = ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,0] = ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,1] = -ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][2,2] = ori_function_L[:, i][3]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,0] = -ori_function_L[:, i][0]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,1] = -ori_function_L[:, i][1]
+# LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3][3,2] = -ori_function_L[:, i][2]
+
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm position' + str(i), optas.sumsqr(pos_fnc_Right(q_var_MPC[:, i])-pos_R[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position' + str(i), optas.sumsqr(pos_fnc_Left(q_var_MPC[:, i])-pos_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i]+q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('distance' + str(i), 0.05 * optas.sumsqr(P[:, i+1] - P[:, i]))
+# if(i<(self.T_MPC -1)):
+# builder_wholebodyMPC.add_cost_term('minimize_velocity2' + str(i), 0.0005 * optas.sumsqr(q_var_MPC[6:self.ndof, i+1] - q_var_MPC[6:self.ndof, i]))
+
+# obstacle_pos = np.asarray([[3.8], [0]])
+# obstacle_radius = 0.75
+# offset = np.asarray([[0.5], [0]])
+# builder_wholebodyMPC.add_geq_inequality_constraint('donkey_obstacle' + str(i), lhs=optas.norm_2(P[0:2, i] + offset - obstacle_pos), rhs=obstacle_radius**2)
+
+# if(i<(self.T_MPC -1)):
+# builder_wholebodyMPC.add_cost_term('Length' + str(i), optas.sumsqr(P[:, i+1]-P[:, i]))
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', P[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', P[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', P[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+
+# builder_wholebodyMPC.add_bound_inequality_constraint('init_p_equal', lhs=init_position_MPC[3:self.ndof] - np.full((15, 1), 0.005), mid=P[3:self.ndof, 0] + p_epsilon[3:self.ndof], rhs=init_position_MPC[3:self.ndof] + np.full((15, 1), 0.005))
+# builder_wholebodyMPC.add_bound_inequality_constraint('init_p_equal2', lhs=init_position_MPC[0:3] - np.full((3, 1), 0.001), mid=P[0:3, 0] + p_epsilon[0:3], rhs=init_position_MPC[0:3] + np.full((3, 1), 0.001))
+# builder_wholebodyMPC.add_cost_term('minimize_p_epsilon', optas.sumsqr(p_epsilon))
+
+# builder_wholebodyMPC.add_inequality_constraint('init_velocity', lhs=self.n * (P[0:3, 1] - P[0:3, 0]), rhs=init_velocity_MPC[0:3])
+
+
+# # get end-effector pose as parameters
+# dpos_R = builder_wholebodyMPC.add_parameter('dpos_R', 3, self.T_MPC)
+# dori_R = builder_wholebodyMPC.add_parameter('dori_R', 4, self.T_MPC)
+# dpos_L = builder_wholebodyMPC.add_parameter('dpos_L', 3, self.T_MPC)
+# dori_L = builder_wholebodyMPC.add_parameter('dori_L', 4, self.T_MPC)
+# # functions of right and left arm positions
+# dpos_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+# dpos_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+# # quaternion functions of two arm end effectors
+# dori_fnc_Right = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_right)
+# dori_fnc_Left = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_left)
+ # define dq function depending on P
+
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0001/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P[:, j+1] - P[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each P[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=self.n * (P[:, i+1] - P[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_L[:, i]))
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P[:, j+2] - 2*P[:, j+1] + P[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+# # optimization cost: close to target
+# builder_wholebodyMPC.add_cost_term('Right_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm linear velocity' + str(i), optas.sumsqr(dpos_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dpos_L[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm angular velocity' + str(i), optas.sumsqr(0.5 * RARM_Matrix_angular2quat[:, i * 3 : (i+1)*3] @ dori_fnc_Right(q_var_MPC[:, i]) @ dq_var_MPC[:, i]-dori_R[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm angular velocity' + str(i), optas.sumsqr(0.5 * LARM_Matrix_angular2quat[:, i * 3 : (i+1)*3] @ dori_fnc_Left(q_var_MPC[:, i]) @ dq_var_MPC[:, i] -dori_L[:, i]))
+
+
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('ipopt', solver_options={'ipopt.print_level': 0, 'print_time': 0})
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0, 'print_time': 0,
+ 'knitro.FeasTol': 1e-6, 'knitro.OptTol': 1e-6, 'knitro.ftol':1e-6,
+ 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1} )
+ self.ti_MPC = 0 # time index of the MPC
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ ### ---------------------------------------------------------
+ # declare ft_sensor subscriber
+# self._ft_right_sub = rospy.Subscriber(
+# "/ft_right/raw/data",
+# WrenchStamped,
+# self.read_ft_sensor_right_data_cb
+# )
+# self._ft_left_sub = rospy.Subscriber(
+# "/ft_left/raw/data",
+# WrenchStamped,
+# self.read_ft_sensor_left_data_cb
+# )
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/chonk/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+# self._joint_sub_base = rospy.Subscriber(
+# "/chonk/donkey_velocity_controller/odom",
+# Odometry,
+# self.read_base_states_cb
+# )
+# self._joint_sub_base = rospy.Subscriber(
+# "/tf",
+# Odometry,
+# self.read_base_states_cb
+# )
+# self._joint_sub_base = rospy.Subscriber(
+# "/chonk/base_pose_ground_truth",
+# Odometry,
+# self.read_base_states_cb
+# )
+ # declare joint publisher
+# self._joint_pub = rospy.Publisher(
+# self._pub_cmd_topic_name,
+# Float64MultiArray,
+# queue_size=10
+# )
+# self._joint_pub = rospy.Publisher(
+# "/chonk/streaming_controller/command",
+# Float64MultiArray,
+# queue_size=10
+# )
+ self._joint_pub = rospy.Publisher(
+ "/chonk/trajectory_controller/command",
+ JointTrajectory,
+ queue_size=10
+ )
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher(
+ "/chonk/donkey_velocity_controller/cmd_vel",
+ Twist,
+ queue_size=10
+ )
+ # subscriber head motion states
+ self._head_sub = rospy.Subscriber(
+ "/chonk/head_states",
+ Float64MultiArray,
+ self.read_head_states_cb
+ )
+ self.head_states = np.array([0., 0.])
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ # initialize action messages
+ self._feedback = CmdChonkPoseFeedback()
+ self._result = CmdChonkPoseResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'cmd_pose',
+ CmdChonkPoseAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([
+ acceped_goal.poseR.position.x,
+ acceped_goal.poseR.position.y,
+ acceped_goal.poseR.position.z
+ ])
+ pos_Left = np.asarray([
+ acceped_goal.poseL.position.x,
+ acceped_goal.poseL.position.y,
+ acceped_goal.poseL.position.z
+ ])
+ ori_Right = np.asarray([
+ acceped_goal.poseR.orientation.x,
+ acceped_goal.poseR.orientation.y,
+ acceped_goal.poseR.orientation.z,
+ acceped_goal.poseR.orientation.w
+ ])
+ ori_Left = np.asarray([
+ acceped_goal.poseL.orientation.x,
+ acceped_goal.poseL.orientation.y,
+ acceped_goal.poseL.orientation.z,
+ acceped_goal.poseL.orientation.w
+ ])
+ # check boundaries of the position
+ if (pos_Right > self._pos_max).any() or (pos_Right < self._pos_min).any():
+ rospy.logwarn("%s: Request aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Right[0], pos_Right[1], pos_Right[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ if (pos_Left > self._pos_max).any() or (pos_Left < self._pos_min).any():
+ rospy.logwarn("%s: Lequest aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Left[0], pos_Left[1], pos_Left[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name,
+ pos_Right[0], pos_Right[1], pos_Right[2],
+ ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2],
+ ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3],
+ acceped_goal.duration
+ )
+ )
+ # read current robot joint positions
+ try:
+ (trans,rot) = self.tf_listener.lookupTransform('/vicon/world', 'vicon/chonk/CHONK', rospy.Time(0))
+ except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+ print("error: cannot find vicon data!!!!")
+ self.base_euler_angle = tf.transformations.euler_from_quaternion([rot[0], rot[1], rot[2], rot[3]])
+ self.q_curr_base = [trans[0], trans[1], self.base_euler_angle[2]]
+
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### optas
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - start_RARM_pos
+ Derivation_RARM_Quat = ori_Right - start_RARM_quat
+ Derivation_LARM_Pos = pos_Left - start_LARM_pos
+ Derivation_LARM_Quat = ori_Left - start_LARM_quat
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._DRARM_ee_Pos_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_RARM_Pos/self.duration)
+ self._LARM_ee_Pos_trajectory = lambda t: start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._DLARM_ee_Pos_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_LARM_Pos/self.duration)
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._DRARM_ee_Quat_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_RARM_Quat/self.duration)
+ self._LARM_ee_Quat_trajectory = lambda t: start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ self._DLARM_ee_Quat_trajectory = lambda t: (30.*((t/self.duration)**2) - 60.*((t/self.duration)**3) +30.*((t/self.duration)**4))*(Derivation_LARM_Quat/self.duration)
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+
+ ### ---------------------------------------------------------
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ self.head_trajectory = lambda _t: self.head_states[0] + (10.*((_t/self.duration)**3) - 15.*((_t/self.duration)**4) + 6.*((_t/self.duration)**5))*(self.head_states[1]-self.head_states[0]) # 5th order
+# print(self.head_states)
+ """ Publish the robot configuration """
+ try:
+ (trans,rot) = self.tf_listener.lookupTransform('/vicon/world', 'vicon/chonk/CHONK', rospy.Time(0))
+ except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+ print("error: cannot find vicon data!!!!")
+ self.base_euler_angle = tf.transformations.euler_from_quaternion([rot[0], rot[1], rot[2], rot[3]])
+ self.q_curr_base = [trans[0], trans[1], self.base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(self.base_euler_angle[2])
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+# """ee mass data in ee frame"""
+# ft_ee_mass_right = X_fromRandP_different(self.rot_ee_right_fnc_global(self.q_curr), self.pos_mass_INee_Right).T @ self.mass_ee_Force
+# ft_ee_mass_left = X_fromRandP_different(self.rot_ee_left_fnc_global(self.q_curr), self.pos_mass_INee_Left).T @ self.mass_ee_Force
+# """Sensor data delete mass influence"""
+# ft_ee_composite_right = ft_ee_right + ft_ee_mass_right
+# ft_ee_composite_left = ft_ee_left + ft_ee_mass_left
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+
+ pos_R_goal = []
+ ori_R_goal = []
+ pos_L_goal = []
+ ori_L_goal = []
+ dpos_R_goal = []
+ dori_R_goal = []
+ dpos_L_goal = []
+ dori_L_goal = []
+
+ for i in range(self.T_MPC):
+# self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+# print(g_rarm_ee_ori[0]**2+g_rarm_ee_ori[1]**2+g_rarm_ee_ori[2]**2 +g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+
+# dg_rarm_ee_pos = self._DRARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# dpos_R_goal.append(dg_rarm_ee_pos.tolist())
+# dg_rarm_ee_ori = self._DRARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# dori_R_goal.append(dg_rarm_ee_ori.tolist())
+# dg_larm_ee_pos = self._DLARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# dpos_L_goal.append(dg_larm_ee_pos.tolist())
+# dg_larm_ee_ori = self._DLARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# dori_L_goal.append(dg_larm_ee_ori.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+# dpos_R_goal.append(dg_rarm_ee_pos.tolist()) # i.e. previous goal
+# dori_R_goal.append(dg_rarm_ee_ori.tolist()) # i.e. previous goal
+# dpos_L_goal.append(dg_rarm_ee_pos.tolist()) # i.e. previous goal
+# dori_L_goal.append(dg_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+# dpos_R_goal = optas.np.array(dpos_R_goal).T
+# dori_R_goal = optas.np.array(dori_R_goal).T
+# dpos_L_goal = optas.np.array(dpos_L_goal).T
+# dori_L_goal = optas.np.array(dori_L_goal).T
+
+
+
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'P': self.curr_MPC})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'P': self.solution_MPC[f'P']})
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal, 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr } )
+# self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal, 't': self.timebyT} )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ P = np.asarray(self.solution_MPC[f'P'])
+
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC
+ n = self.T_MPC -1
+ q_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * P[:, j]
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P[:, j+1] - P[:, j])
+# q_next = P[:, 0]
+# dq_next = self.n * (P[:, 1] - P[:, 0])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+# self._msg.data[0:12] = q_next[-12:]
+# self._msg.data[12:15] = q_next[3:6]
+# self.eva_point.positions = q_next[-self.ndof_position_control:].tolist()
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ q_next[5] = self.head_trajectory(self._t[self._idx])
+# print(self.head_trajectory(self._t[self._idx]))
+ self.eva_trajectory.points[0].positions = q_next[-self.ndof_position_control:].tolist()
+
+# print(q_next[-self.ndof_position_control:])
+
+# self._msg.data = [float('%.3f' % x) for x in self._msg.data]
+# self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2] * 6.05
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_ft_sensor_right_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_right = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_ft_sensor_left_data_cb(self, msg):
+ """ paranet to child: the force/torque from robot to ee"""
+ self.ft_left = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+# def read_base_states_cb(self, msg):
+# base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+# self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+# self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+# self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+# self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+# self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+# self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def read_head_states_cb(self, msg):
+ self.head_states = msg.data
+
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD.py
new file mode 100755
index 0000000..5094acb
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD.py
@@ -0,0 +1,732 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 3000;
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+ #####################################################################################
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+ ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+ pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+ ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual_local = builder_wholebodyMPC.add_parameter('F_ext_Right_actual_local', 3)
+ F_ext_Left_actual_local = builder_wholebodyMPC.add_parameter('F_ext_Left_actual_local', 3)
+# F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+# F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ self.pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ self.ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.rotation_fnc_Right = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rotation_fnc_Left = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+# Global_X_ee_Right = optas.casadi.SX(np.zeros((6, 6)))
+# Global_X_ee_Left = optas.casadi.SX(np.zeros((6, 6)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual_local + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual_local + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+# F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+# F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+# Global_X_ee_right[0:3, 0:3] = self.rotation_fnc_Right(init_position_MPC)
+# Global_X_ee_right[3:6, 3:6] = self.rotation_fnc_Right(init_position_MPC)
+# Global_X_ee_left[0:3, 0:3] = self.rotation_fnc_Left(init_position_MPC)
+# Global_X_ee_left[3:6, 3:6] = self.rotation_fnc_Left(init_position_MPC)
+ for i in range(self.T_MPC):
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('Two_arm orientation parallel' + str(i), 0.1*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i]).T @ self.ori_fnc_Left(q_var_MPC[:, i])))
+
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i]))
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Right(init_position_MPC) @ (F_ext_Right_var_MPC[:, i] - F_ext_Right_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Left(init_position_MPC) @ (F_ext_Left_var_MPC[:, i] - F_ext_Left_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr( (self.rotation_fnc_Right(init_position_MPC) @ F_ext_Right_var_MPC[:, i] + self.rotation_fnc_Left(init_position_MPC) @ F_ext_Left_var_MPC[:, i])[1] - m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+# builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[1, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[1, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[2, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[2, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ ddlower = np.array([-1.5, -1.5, -1.5]); ddupper = np.array([1.5, 1.5, 1.5]);
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ if(i<(self.T_MPC -2)):
+ name = 'control_point_dderiv_' + str(i) + '_bound' # add acceleration constraint for Q[0:3, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=ddlower, mid=(1./self.duration_MPC)**2 * self.n * (self.n - 1) * (Q[0:3, i+2]- 2*Q[0:3, i+1] + Q[0:3, i]), rhs=ddupper)
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+
+ #########################################################################################
+ # setup solver
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ solver_options = {'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5,'knitro.ftol':1e-5,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1 }
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options)
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare ft_sensor subscriber
+# self._ft_right_sub = rospy.Subscriber("/ft_right/raw/data", WrenchStamped, self.read_ft_sensor_right_data_cb)
+# self._ft_left_sub = rospy.Subscriber("/ft_left/raw/data", WrenchStamped, self.read_ft_sensor_left_data_cb)
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+# self._joint_pub = rospy.Publisher(self._pub_cmd_topic_name, Float64MultiArray, queue_size=10)
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+# self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+# self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_local_right = rospy.Subscriber("/chonk/sensor_ft_local_right", Float64MultiArray, self.read_right_ee_grasp_ft_local_data_cb)
+ self._sensor_ft_sub_local_left = rospy.Subscriber("/chonk/sensor_ft_local_left", Float64MultiArray, self.read_left_ee_grasp_ft_local_data_cb)
+ # declare two arm ee grasp actual force publisher
+# self._acc_sub = rospy.Subscriber("/chonk/acc_box", Float64MultiArray, self.read_box_acc_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, pos_Right[0], pos_Right[1], pos_Right[2], ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2], ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ time = rospy.get_time()
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self.ti_MPC = 0 # time index of the MPC
+ self._idx += 1
+ pos_R_goal = []; ori_R_goal = []; pos_L_goal = []; ori_L_goal = []; force_R_goal = []; force_L_goal = [];
+# ddpos_box_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+# g_box_ddpos = self._RARM_ee_ddPos_trajectory(self.ti_MPC).flatten()
+# ddpos_box_goal.append(g_box_ddpos.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+# ddpos_box_goal.append(g_box_ddpos.tolist())
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+ print(force_R_goal)
+# ddpos_box_goal = optas.np.array(ddpos_box_goal).T
+
+# print(ddpos_box_goal)
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+# self.Derivation_RARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1] - pos_R_goal[1, 0]
+# self.Derivation_LARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1] - pos_L_goal[1, 0]
+
+ self.G_Rotation_ee_right = self.rotation_fnc_Right(self.q_curr)
+ self.G_Rotation_ee_left = self.rotation_fnc_Left(self.q_curr)
+
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+
+# print(self.G_I_ee_r_conventional[3:6, 3:6])
+# print(self.G_I_ee_l_conventional[3:6, 3:6])
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'inertia_Right': self.G_I_ee_r_conventional[3:6, 3:6], 'inertia_Left': self.G_I_ee_l_conventional[3:6, 3:6],
+# 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_actual_local': self.F_ext_Left[3:6],
+ 'F_ext_Right_actual_local': self.F_ext_local_Right[3:6], 'F_ext_Left_actual_local': self.F_ext_local_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box} )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ q_next = np.zeros(self.ndof); p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += (1./self.duration_MPC) * self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+
+ ddq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-2):
+ ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+
+# self.acc_box = 0.5*(self.pos_Jac_fnc_Right(q_next) + self.pos_Jac_fnc_Left(q_next)) @ ddq_next
+# F_r = np.zeros(3)
+# F_l = np.zeros(3)
+# Delta_p_r = np.zeros(3)
+# dDelta_p_r = np.zeros(3)
+# ddDelta_p_r = np.zeros(3)
+# Delta_p_l = np.zeros(3)
+# dDelta_p_l = np.zeros(3)
+# ddDelta_p_l = np.zeros(3)
+# for j in range(self.T_MPC):
+# Delta_p_r += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+# Delta_p_l += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Left[:, j]
+# for j in range(self.T_MPC-1):
+# dDelta_p_r += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P_Right[:, j+1] - P_Right[:, j])
+# dDelta_p_l += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P_Left[:, j+1] - P_Left[:, j])
+# for j in range(self.T_MPC-2):
+# ddDelta_p_r += self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+# ddDelta_p_l += self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+# #####################################################################################
+# F_r = self.F_ext_Right[3:6] + self.K_Right @ Delta_p_r
+# F_l = self.F_ext_Left[3:6] + self.K_Left @ Delta_p_l
+# F_r = self.F_ext_Right[3:6] + self.G_I_ee_r_conventional[3:6, 3:6] @ ddDelta_p_r + self.K_Right @ Delta_p_r + self.D_Right @ dDelta_p_r
+# F_l = self.F_ext_Left[3:6] + self.G_I_ee_l_conventional[3:6, 3:6] @ ddDelta_p_l + self.K_Left @ Delta_p_l + self.D_Left @ dDelta_p_l
+
+# print(F_r)
+# print(F_l)
+ print(self.G_Rotation_ee_right @ P_Right[:,1])
+ print(self.G_Rotation_ee_left @ P_Left[:,1])
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[2] = np.asarray(self.pos_fnc_Right(q_next)).T[0][1] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[2] = np.asarray(self.pos_fnc_Left(q_next)).T[0][1] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0][1]
+
+# self.Derivation_RARM_pos_start[1] = pos_R_goal[1, 0] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1]
+# self.Derivation_LARM_pos_start[1] = pos_L_goal[1, 0] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1]
+
+# print(self.Derivation_RARM_pos_start)
+# print(self.Derivation_LARM_pos_start)
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ print(rospy.get_time()-time)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+# def read_ft_sensor_right_data_cb(self, msg):
+# self.ft_right = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+# def read_ft_sensor_left_data_cb(self, msg):
+# self.ft_left = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+# def read_right_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+# def read_left_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+ def read_right_ee_grasp_ft_local_data_cb(self, msg):
+ self.F_ext_local_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5]])
+
+ def read_left_ee_grasp_ft_local_data_cb(self, msg):
+ self.F_ext_local_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5] ])
+
+# def read_box_acc_data_cb(self, msg):
+# self.acc_box = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.py
new file mode 100755
index 0000000..e10e35c
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_longhorizon.py
@@ -0,0 +1,951 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+ self.dt_MPC_planner = 0.1 # time step
+ self.T_MPC_planner = 10 # T is number of time steps
+ self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_Right = builder_wholebodyMPC_planner.add_decision_variables('R_Right', 3, self.T_MPC_planner)
+ R_Left = builder_wholebodyMPC_planner.add_decision_variables('R_Left', 3, self.T_MPC_planner)
+ R_middle = builder_wholebodyMPC_planner.add_decision_variables('R_middle', 3, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+
+ t = builder_wholebodyMPC_planner.add_parameter('t', self.T_MPC_planner) # time
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+ # Add parameters
+# init_r_position_middle = builder_wholebodyMPC_planner.add_parameter('init_r_position_middle', 2)
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 2)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 2)
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3, self.T_MPC_planner)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3, self.T_MPC_planner)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+ r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Right[:, j]
+ r_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Left[:, j]
+ # optimization cost: close to target
+ builder_wholebodyMPC_planner.add_cost_term('Right_middle_x' + str(i), optas.sumsqr(r_middle_var_MPC[0, i] - 0.5*(pos_R[0, i] + pos_L[0, i])))
+ builder_wholebodyMPC_planner.add_cost_term('Right_middle_y' + str(i), 10*optas.sumsqr(r_middle_var_MPC[1, i] - 0.5*(pos_R[1, i] + pos_L[1, i])))
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - r_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_RARM_var_MPC[0, i], rhs=r_LARM_var_MPC[0, i])
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_middle_z', r_middle_var_MPC[2, :], rhs=0.5*(pos_R[2, :]+pos_L[2, :]))
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_xy', r_RARM_var_MPC[0:2, :], rhs=r_middle_var_MPC[0:2, :] - 0.5*(pos_L[0:2, :]-pos_R[0:2, :]))
+ builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_xy', r_LARM_var_MPC[0:2, :], rhs=r_middle_var_MPC[0:2, :] + 0.5*(pos_L[0:2, :]-pos_R[0:2, :]))
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_z', r_RARM_var_MPC[2, :], rhs=r_middle_var_MPC[2, :])
+ builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_z', r_LARM_var_MPC[2, :], rhs=r_middle_var_MPC[2, :])
+# builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_z', R_Right[2, :], rhs=pos_R[2, :])
+# builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_z', R_Left[2, :], rhs=pos_L[2, :])
+
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', optas.sumsqr(r_ep))
+
+ # add position constraint at the beginning state
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[0:2, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+
+
+ dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ w_dr = 0.05/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+ dr_middle_var_MPC[:, i] += (1./self.duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+# dr_RARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Right[:, j+1] - R_Right[:, j])
+# dr_LARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Left[:, j+1] - R_Left[:, j])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_right' + str(i), w_dq * optas.sumsqr(dr_RARM_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_left' + str(i), w_dq * optas.sumsqr(dr_LARM_var_MPC[:, i]))
+
+ ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ w_ddr = 0.05/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+ ddr_middle_var_MPC[:, i] += (1./self.duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+# ddr_RARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Right[:, j+2] - 2*R_Right[:, j+1] + R_Right[:, j])
+# ddr_LARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Left[:, j+2] - 2*R_Left[:, j+1] + R_Left[:, j])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_right' + str(i), w_ddr * optas.sumsqr(ddr_RARM_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_left' + str(i), w_ddr * optas.sumsqr(ddr_LARM_var_MPC[:, i]))
+
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.FeasTol': 1e-4, 'knitro.OptTol': 1e-4,
+ 'knitro.ftol':1e-4,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+ self.time_linspace_planner = np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner)
+ self.timebyT_planner = np.asarray(self.time_linspace_planner)/self.duration_MPC_planner
+ ### ---------------------------------------------------------
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 3000;
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+ #####################################################################################3
+ # get end-effector pose as parameters
+# pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+# ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+# pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+# ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+
+ pos_R_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ pos_L_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ R_Right_reasonal = builder_wholebodyMPC.add_parameter('R_Right_reasonal', 3, self.T_MPC_planner)
+ R_Left_reasonal = builder_wholebodyMPC.add_parameter('R_Left_reasonal', 3, self.T_MPC_planner)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+
+ fac = self.duration_MPC/self.duration_MPC_planner
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t[i]*fac)**j * (1-t[i]*fac)**(self.n_planner-j) * R_Right_reasonal[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t[i]*fac)**j * (1-t[i]*fac)**(self.n_planner-j) * R_Left_reasonal[:, j]
+
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual_local = builder_wholebodyMPC.add_parameter('F_ext_Right_actual_local', 3)
+ F_ext_Left_actual_local = builder_wholebodyMPC.add_parameter('F_ext_Left_actual_local', 3)
+# F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+# F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ self.pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ self.ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.rotation_fnc_Right = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rotation_fnc_Left = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual_local + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual_local + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+# F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+# F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Two_arm orientation parallel' + str(i), 0.1*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i]).T @ self.ori_fnc_Left(q_var_MPC[:, i])))
+
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i]))
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Right(init_position_MPC) @ (F_ext_Right_var_MPC[:, i] - F_ext_Right_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Left(init_position_MPC) @ (F_ext_Left_var_MPC[:, i] - F_ext_Left_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr( (self.rotation_fnc_Right(init_position_MPC) @ F_ext_Right_var_MPC[:, i] + self.rotation_fnc_Left(init_position_MPC) @ F_ext_Left_var_MPC[:, i])[1] - m_box * ddpos_box_goal[:, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+# builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[1, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[1, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[2, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[2, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+
+ #########################################################################################
+ # setup solver
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5,
+ 'knitro.ftol':1e-5, 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+ 'knitro.maxtime_real': 1.6e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4,
+ 'knitro.bar_penaltycons': 1, 'knitro.bar_penaltyrule':2,
+ 'knitro.bar_switchrule':2, 'knitro.linesearch': 1})
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare ft_sensor subscriber
+# self._ft_right_sub = rospy.Subscriber("/ft_right/raw/data", WrenchStamped, self.read_ft_sensor_right_data_cb)
+# self._ft_left_sub = rospy.Subscriber("/ft_left/raw/data", WrenchStamped, self.read_ft_sensor_left_data_cb)
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+# self._joint_pub = rospy.Publisher(self._pub_cmd_topic_name, Float64MultiArray, queue_size=10)
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+# self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+# self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_local_right = rospy.Subscriber("/chonk/sensor_ft_local_right", Float64MultiArray, self.read_right_ee_grasp_ft_local_data_cb)
+ self._sensor_ft_sub_local_left = rospy.Subscriber("/chonk/sensor_ft_local_left", Float64MultiArray, self.read_left_ee_grasp_ft_local_data_cb)
+ # declare two arm ee grasp actual force publisher
+# self._acc_sub = rospy.Subscriber("/chonk/acc_box", Float64MultiArray, self.read_box_acc_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, pos_Right[0], pos_Right[1], pos_Right[2], ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2], ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ # main execution
+ if(self._idx < self._steps):
+ time = rospy.get_time()
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self.ti_MPC_planner = 0 # time index of the MPC
+ self._idx += 1
+ pos_R_goal = []
+ pos_L_goal = []
+
+ for i in range(self.T_MPC_planner):
+ if(self.ti_MPC_planner <= self.duration):
+ self.ti_MPC_planner = self._t[self._idx-1] + self.dt_MPC_planner*i
+ if(self.ti_MPC_planner > self.duration):
+ self.ti_MPC_planner = self.duration
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC_planner).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC_planner).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_actual_Right = np.array(self.pos_fnc_Right(self.q_curr))[0:2]
+ r_actual_Left = np.array(self.pos_fnc_Left(self.q_curr))[0:2]
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC planner
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_middle': 0.5*(pos_R_goal+pos_L_goal), f'R_Right': pos_R_goal,
+ f'R_Left': pos_L_goal, f'r_ep': np.zeros(self.T_MPC_planner) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_middle': self.solution_MPC_planner[f'R_middle'],
+ f'R_Right': self.solution_MPC_planner[f'R_Right'],
+ f'R_Left': self.solution_MPC_planner[f'R_Left'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'] })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': pos_R_goal, 'pos_L': pos_L_goal,
+ 't': self.timebyT_planner,
+ 'init_r_position_Right': r_actual_Right,
+ 'init_r_position_Left': r_actual_Left} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_Right = np.asarray(self.solution_MPC_planner[f'R_Right'])
+ R_Left = np.asarray(self.solution_MPC_planner[f'R_Left'])
+
+ ### -----------------------------------------------------------
+ self.ti_MPC = 0
+ force_R_goal = []
+ force_L_goal = []
+ ori_R_goal = []
+ ori_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+
+# if(self._idx < self._steps):
+# if(self._correct_mux_selection):
+# # increment idx (in here counts starts with 1)
+# self.ti_MPC = 0 # time index of the MPC
+# self._idx += 1
+# pos_R_goal = []; ori_R_goal = []; pos_L_goal = []; ori_L_goal = []; force_R_goal = []; force_L_goal = [];
+## ddpos_box_goal = []
+# for i in range(self.T_MPC):
+# if(self.ti_MPC <= self.duration):
+# self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+# if(self.ti_MPC > self.duration):
+# self.ti_MPC = self.duration
+# try:
+# g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# pos_R_goal.append(g_rarm_ee_pos.tolist())
+# g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+## g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+# ori_R_goal.append(g_rarm_ee_ori.tolist())
+# g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# pos_L_goal.append(g_larm_ee_pos.tolist())
+# g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+## g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+# ori_L_goal.append(g_larm_ee_ori.tolist())
+# g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+# force_R_goal.append(g_rarm_ee_force.tolist())
+# g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+# force_L_goal.append(g_larm_ee_force.tolist())
+## g_box_ddpos = self._RARM_ee_ddPos_trajectory(self.ti_MPC).flatten()
+## ddpos_box_goal.append(g_box_ddpos.tolist())
+# except ValueError:
+# pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+# ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+# pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+# ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+# force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+# force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+## ddpos_box_goal.append(g_box_ddpos.tolist())
+# pos_R_goal = optas.np.array(pos_R_goal).T
+# ori_R_goal = optas.np.array(ori_R_goal).T
+# pos_L_goal = optas.np.array(pos_L_goal).T
+# ori_L_goal = optas.np.array(ori_L_goal).T
+# force_R_goal = optas.np.array(force_R_goal).T
+# force_L_goal = optas.np.array(force_L_goal).T
+## ddpos_box_goal = optas.np.array(ddpos_box_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+# self.Derivation_RARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1] - pos_R_goal[1, 0]
+# self.Derivation_LARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1] - pos_L_goal[1, 0]
+
+ self.G_Rotation_ee_right = self.rotation_fnc_Right(self.q_curr)
+ self.G_Rotation_ee_left = self.rotation_fnc_Left(self.q_curr)
+
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+
+# print(self.G_I_ee_r_conventional[3:6, 3:6])
+# print(self.G_I_ee_l_conventional[3:6, 3:6])
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ self.solver_wholebodyMPC.reset_parameters({'R_Right_reasonal': R_Right, 'R_Left_reasonal': R_Left,
+ 'ori_R_reasonal': ori_R_goal, 'ori_L_reasonal': ori_L_goal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'inertia_Right': self.G_I_ee_r_conventional[3:6, 3:6], 'inertia_Left': self.G_I_ee_l_conventional[3:6, 3:6],
+ 'F_ext_Right_actual_local': self.F_ext_local_Right[3:6], 'F_ext_Left_actual_local': self.F_ext_local_Left[3:6],
+# 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_Left_actual': self.F_ext_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box} )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ q_next = np.zeros(self.ndof); p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += (1./self.duration_MPC) * self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+
+ ddq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-2):
+ ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+
+# self.acc_box = 0.5*(self.pos_Jac_fnc_Right(q_next) + self.pos_Jac_fnc_Left(q_next)) @ ddq_next
+# F_r = np.zeros(3)
+# F_l = np.zeros(3)
+# Delta_p_r = np.zeros(3)
+# dDelta_p_r = np.zeros(3)
+# ddDelta_p_r = np.zeros(3)
+# Delta_p_l = np.zeros(3)
+# dDelta_p_l = np.zeros(3)
+# ddDelta_p_l = np.zeros(3)
+# for j in range(self.T_MPC):
+# Delta_p_r += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+# Delta_p_l += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Left[:, j]
+# for j in range(self.T_MPC-1):
+# dDelta_p_r += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P_Right[:, j+1] - P_Right[:, j])
+# dDelta_p_l += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P_Left[:, j+1] - P_Left[:, j])
+# for j in range(self.T_MPC-2):
+# ddDelta_p_r += self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+# ddDelta_p_l += self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+# #####################################################################################
+# F_r = self.F_ext_Right[3:6] + self.K_Right @ Delta_p_r
+# F_l = self.F_ext_Left[3:6] + self.K_Left @ Delta_p_l
+# F_r = self.F_ext_Right[3:6] + self.G_I_ee_r_conventional[3:6, 3:6] @ ddDelta_p_r + self.K_Right @ Delta_p_r + self.D_Right @ dDelta_p_r
+# F_l = self.F_ext_Left[3:6] + self.G_I_ee_l_conventional[3:6, 3:6] @ ddDelta_p_l + self.K_Left @ Delta_p_l + self.D_Left @ dDelta_p_l
+
+# print(F_r)
+# print(F_l)
+
+ # read current robot joint positions
+# self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+# self.Derivation_RARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q_next)).T[0][1] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1]
+# self.Derivation_LARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q_next)).T[0][1] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1]
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[1] = np.asarray(self.pos_fnc_Right(q_next)).T[0][1] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[1] = np.asarray(self.pos_fnc_Left(q_next)).T[0][1] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0][1]
+
+
+# self.Derivation_RARM_pos_start[1] = pos_R_goal[1, 0] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1]
+# self.Derivation_LARM_pos_start[1] = pos_L_goal[1, 0] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1]
+
+# print(self.Derivation_RARM_pos_start)
+# print(self.Derivation_LARM_pos_start)
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ print(rospy.get_time() - time)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+# def read_ft_sensor_right_data_cb(self, msg):
+# self.ft_right = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+# def read_ft_sensor_left_data_cb(self, msg):
+# self.ft_left = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+# def read_right_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+# def read_left_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+ def read_right_ee_grasp_ft_local_data_cb(self, msg):
+ self.F_ext_local_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5]])
+
+ def read_left_ee_grasp_ft_local_data_cb(self, msg):
+ self.F_ext_local_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5] ])
+
+# def read_box_acc_data_cb(self, msg):
+# self.acc_box = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.py
new file mode 100755
index 0000000..5257001
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory.py
@@ -0,0 +1,936 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 40)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+# self.dt_MPC_planner = 0.1 # time step
+ self.T_MPC_planner = 13 # T is number of time steps
+# self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_Right = builder_wholebodyMPC_planner.add_decision_variables('R_Right', 3, self.T_MPC_planner)
+ R_Left = builder_wholebodyMPC_planner.add_decision_variables('R_Left', 3, self.T_MPC_planner)
+# R_middle = builder_wholebodyMPC_planner.add_decision_variables('R_middle', 3, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+ r_ep_start = builder_wholebodyMPC_planner.add_decision_variables('r_ep_start', 12)
+
+ duration_MPC_planner = builder_wholebodyMPC_planner.add_parameter('duration_MPC_planner', 1)
+
+ t = np.linspace(0., 1., self.T_MPC_planner)
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+ # Add parameters
+# init_r_position_middle = builder_wholebodyMPC_planner.add_parameter('init_r_position_middle', 2)
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 3)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 3)
+
+ init_dr_position_Right = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Right', 3)
+ init_dr_position_Left = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Left', 3)
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+# r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Right[:, j]
+ r_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Left[:, j]
+ r_middle_var_MPC[:, i] = 0.5*(r_RARM_var_MPC[:, i]+r_LARM_var_MPC[:, i])
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_z' + str(i), optas.sumsqr(r_RARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_z' + str(i), optas.sumsqr(r_LARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_equality_constraint('middle' +str(i), r_middle_var_MPC[:, i], rhs=0.5*(r_RARM_var_MPC[:, i]+r_LARM_var_MPC[:, i]))
+
+
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - r_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_RARM_var_MPC[0, i], rhs=r_LARM_var_MPC[0, i])
+
+ # optimization cost: close to target
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_x', optas.sumsqr(r_middle_var_MPC[0, -1] - 0.5*(pos_R[0] + pos_L[0])))
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_y', 10*optas.sumsqr(r_middle_var_MPC[1, -1] - 0.5*(pos_R[1] + pos_L[1])))
+# builder_wholebodyMPC_planner.add_equality_constraint('Final_middle', r_middle_var_MPC[:, self.T_MPC_planner-1], rhs=0.5*(pos_R+pos_L))
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_arm', r_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_arm', r_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_L)
+
+
+
+
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+# builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2)
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', 10*optas.sumsqr(r_ep))
+
+ # add position constraint at the beginning state
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[:, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Right', R_Right[:, 0], rhs=init_r_position_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Left', R_Left[:, 0], rhs=init_r_position_Left)
+
+ for i in range(self.T_MPC_planner):
+ if(i<(self.T_MPC_planner -1)):
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance_x' + str(i), 50 * optas.sumsqr(R_Right[0, i+1] - R_Right[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance_y' + str(i), 50 * optas.sumsqr(R_Right[1, i+1] - R_Right[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance_z' + str(i), 50 * optas.sumsqr(R_Right[2, i+1] - R_Right[2, i]))
+
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance_x' + str(i), 50 * optas.sumsqr(R_Left[0, i+1] - R_Left[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance_y' + str(i), 50 * optas.sumsqr(R_Left[1, i+1] - R_Left[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance_z' + str(i), 50 * optas.sumsqr(R_Left[2, i+1] - R_Left[2, i]))
+
+ if(i<(self.T_MPC_planner -2)):
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance_x' + str(i), 50 * optas.sumsqr(R_Right[0, i+2]-2*R_Right[0, i+1] + R_Right[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance_y' + str(i), 50 * optas.sumsqr(R_Right[1, i+2]-2*R_Right[1, i+1] + R_Right[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance_z' + str(i), 50 * optas.sumsqr(R_Right[2, i+2]-2*R_Right[2, i+1] + R_Right[2, i]))
+
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_x' + str(i), 50 * optas.sumsqr(R_Left[0, i+2]-2*R_Left[0, i+1] + R_Left[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_y' + str(i), 50 * optas.sumsqr(R_Left[1, i+2]-2*R_Left[1, i+1] + R_Left[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_z' + str(i), 50 * optas.sumsqr(R_Left[2, i+2]-2*R_Left[2, i+1] + R_Left[2, i]))
+
+
+# dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ w_dr = duration_MPC_planner**2 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+# dr_middle_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+ dr_RARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Right[:, j+1] - R_Right[:, j])
+ dr_LARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Left[:, j+1] - R_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_right' + str(i), w_dr * optas.sumsqr(dr_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_left' + str(i), w_dr * optas.sumsqr(dr_LARM_var_MPC[:, i]))
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_middle', dr_middle_var_MPC[:, 0], rhs=0.5*(init_dr_position_Right + init_dr_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Right', dr_RARM_var_MPC[0:2, 0], rhs=init_dr_position_Right[0:2])
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Left', dr_LARM_var_MPC[0:2, 0], rhs=init_dr_position_Left[0:2])
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dr_middle', dr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_right', dr_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_left', dr_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+
+ lower_velocity = np.array([-0.9, -0.9, -0.5]); upper_velocity = np.array([0.9, 0.9, 0.5]);
+ for i in range(self.T_MPC_planner-1):
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity-r_ep_start[0:3], mid=(1./duration_MPC_planner) * self.n_planner * (R_Right[:, i+1] - R_Right[:, i]), rhs=upper_velocity+r_ep_start[0:3])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity-r_ep_start[3:6], mid=(1./duration_MPC_planner) * self.n_planner * (R_Left[:, i+1] - R_Left[:, i]), rhs=upper_velocity+r_ep_start[3:6])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_Right[:, i+1] - R_Right[:, i]), rhs=upper_velocity)
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_Left[:, i+1] - R_Left[:, i]), rhs=upper_velocity)
+
+# ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ w_ddr = duration_MPC_planner**4 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+# ddr_middle_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+ ddr_RARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Right[:, j+2] - 2*R_Right[:, j+1] + R_Right[:, j])
+ ddr_LARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Left[:, j+2] - 2*R_Left[:, j+1] + R_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_right' + str(i), w_ddr * optas.sumsqr(ddr_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_left' + str(i), w_ddr * optas.sumsqr(ddr_LARM_var_MPC[:, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_middle', ddr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_right', ddr_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_left', ddr_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+
+ lower_acceleration = np.array([-1., -1., -0.5]); upper_acceleration = np.array([1., 1., 0.5]);
+ for i in range(self.T_MPC_planner-2):
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration-r_ep_start[6:9], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Right[:, i+2] - 2 * R_Right[:, i+1] + R_Right[:, i]), rhs=upper_acceleration+r_ep_start[6:9])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration-r_ep_start[9:12], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Left[:, i+2] - 2 * R_Left[:, i+1] + R_Left[:, i]), rhs=upper_acceleration+r_ep_start[9:12])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Right[:, i+2] - 2 * R_Right[:, i+1] + R_Right[:, i]), rhs=upper_acceleration)
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Left[:, i+2] - 2 * R_Left[:, i+1] + R_Left[:, i]), rhs=upper_acceleration)
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right1', R_Right[:,8], rhs=R_Right[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right2', R_Right[:,9], rhs=R_Right[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left1', R_Left[:,8], rhs=R_Left[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left2', R_Left[:,9], rhs=R_Left[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_right', R_Right[:,self.T_MPC_planner-4], rhs=R_Right[:,self.T_MPC_planner-1])
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_left', R_Left[:,self.T_MPC_planner-4], rhs=R_Left[:,self.T_MPC_planner-1])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep_start', 10*optas.sumsqr(r_ep_start))
+
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.act_qpalg': 1,
+# 'knitro.FeasTol': 5e-4, 'knitro.OptTol': 5e-4, 'knitro.ftol':5e-4,
+ 'knitro.algorithm':3, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+
+ ### ---------------------------------------------------------
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 3000;
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+ #####################################################################################3
+ # get end-effector pose as parameters
+# pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+# ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+# pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+# ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+
+# pos_R_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+# pos_L_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ pos_R_reasonal = builder_wholebodyMPC.add_parameter('pos_R_reasonal', 3, self.T_MPC)
+ pos_L_reasonal = builder_wholebodyMPC.add_parameter('pos_L_reasonal', 3, self.T_MPC)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual_local = builder_wholebodyMPC.add_parameter('F_ext_Right_actual_local', 3)
+ F_ext_Left_actual_local = builder_wholebodyMPC.add_parameter('F_ext_Left_actual_local', 3)
+# F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+# F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ self.pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ self.ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.rotation_fnc_Right = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rotation_fnc_Left = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual_local + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual_local + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+# F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+# F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Two_arm orientation parallel' + str(i), 5*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i]).T @ self.ori_fnc_Left(q_var_MPC[:, i])))
+
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), 2*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), 2*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i]))
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Right(init_position_MPC) @ (F_ext_Right_var_MPC[:, i] - F_ext_Right_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Left(init_position_MPC) @ (F_ext_Left_var_MPC[:, i] - F_ext_Left_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr( (self.rotation_fnc_Right(init_position_MPC) @ F_ext_Right_var_MPC[:, i] + self.rotation_fnc_Left(init_position_MPC) @ F_ext_Left_var_MPC[:, i]) - m_box * ddpos_box_goal[:, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), 10*optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+# builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[1, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[1, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[2, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[2, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = self.duration_MPC**2 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = self.duration_MPC**4 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+
+ #########################################################################################
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0,
+ 'print_time': 0,
+# 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5, 'knitro.ftol':1e-5,
+ 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4,
+ 'knitro.bar_penaltycons': 1, 'knitro.bar_penaltyrule':2,
+ 'knitro.bar_switchrule':2, 'knitro.linesearch': 1})
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+# self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+# self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_local_right = rospy.Subscriber("/chonk/sensor_ft_local_right", Float64MultiArray, self.read_right_ee_grasp_ft_local_data_cb)
+ self._sensor_ft_sub_local_left = rospy.Subscriber("/chonk/sensor_ft_local_left", Float64MultiArray, self.read_left_ee_grasp_ft_local_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ self.pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ self.pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ self.ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ self.ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, self.pos_Right[0], self.pos_Right[1], self.pos_Right[2], self.ori_Right[0], self.ori_Right[1], self.ori_Right[2], self.ori_Right[3],
+ self.pos_Left[0], self.pos_Left[1], self.pos_Left[2], self.ori_Left[0], self.ori_Left[1], self.ori_Left[2], self.ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self.duration_MPC_planner = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = self.pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = self.ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = self.pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = self.ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ # main execution
+ if(self._idx < self._steps):
+ time_begin = rospy.get_time()
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_actual_Right = np.array(self.pos_fnc_Right(self.q_curr))[0:3]
+ r_actual_Left = np.array(self.pos_fnc_Left(self.q_curr))[0:3]
+
+ dr_actual_Right = np.asarray(self.pos_Jac_fnc_Right(self.q_curr)) @ self.dq_curr
+ dr_actual_Left = np.asarray(self.pos_Jac_fnc_Left(self.q_curr)) @ self.dq_curr
+
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC planner
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ pos_R_goal = []; pos_L_goal = [];
+ _t = np.asarray(np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner))
+ for i in range(self.T_MPC_planner):
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': pos_R_goal,
+ f'R_Left': pos_L_goal, f'r_ep': np.zeros(self.T_MPC_planner),
+# f'r_ep_start': np.zeros(12)
+ })
+# self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': np.zeros((3, self.T_MPC_planner)),
+# f'R_Left': np.zeros((3, self.T_MPC_planner)), f'r_ep': np.zeros(self.T_MPC_planner) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': self.solution_MPC_planner[f'R_Right'],
+ f'R_Left': self.solution_MPC_planner[f'R_Left'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'],
+# f'r_ep_start': self.solution_MPC_planner[f'r_ep_start']
+ })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': self.pos_Right, 'pos_L': self.pos_Left,
+ 'init_r_position_Right': r_actual_Right,
+ 'init_r_position_Left': r_actual_Left,
+ 'init_dr_position_Right': dr_actual_Right,
+ 'init_dr_position_Left': dr_actual_Left,
+ 'duration_MPC_planner': self.duration_MPC_planner} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_Right = np.asarray(self.solution_MPC_planner[f'R_Right'])
+ R_Left = np.asarray(self.solution_MPC_planner[f'R_Left'])
+
+ pos_R_reasonal = np.zeros((3, self.T_MPC))
+ pos_L_reasonal = np.zeros((3, self.T_MPC))
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if((self.ti_MPC - self._t[self._idx-1]) <= self.duration_MPC_planner and self.duration_MPC_planner>= 2./self._freq):
+ t_nomalized = (self.ti_MPC - self._t[self._idx-1])/self.duration_MPC_planner
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Left[:, j]
+ else:
+ t_nomalized = 1.
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Left[:, j]
+# print(self._t[self._idx-1])
+# print(self.duration_MPC_planner)
+# print(R_Right[2, :])
+# print(pos_R_reasonal[2, :])
+# print('left')
+# print(R_Left[2, :])
+# print(pos_L_reasonal[2, :])
+ ### -----------------------------------------------------------
+ self.ti_MPC = 0
+ force_R_goal = []
+ force_L_goal = []
+ ori_R_goal = []
+ ori_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ self.G_Rotation_ee_right = self.rotation_fnc_Right(self.q_curr)
+ self.G_Rotation_ee_left = self.rotation_fnc_Left(self.q_curr)
+
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R_reasonal': pos_R_reasonal, 'pos_L_reasonal': pos_L_reasonal,
+ 'ori_R_reasonal': ori_R_goal, 'ori_L_reasonal': ori_L_goal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'inertia_Right': self.G_I_ee_r_conventional[3:6, 3:6], 'inertia_Left': self.G_I_ee_l_conventional[3:6, 3:6],
+ 'F_ext_Right_actual_local': self.F_ext_local_Right[3:6], 'F_ext_Left_actual_local': self.F_ext_local_Left[3:6],
+# 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_Left_actual': self.F_ext_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box } )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ self.q_next = np.zeros(self.ndof); p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ self.q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += (1./self.duration_MPC) * self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ self.dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ self.dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+
+ self.ddq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-2):
+ self.ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[1] = np.asarray(self.pos_fnc_Right(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[1] = np.asarray(self.pos_fnc_Left(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0][1]
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., self.dq_next[2]])
+ Global_v_b = np.asarray([self.dq_next[0], self.dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+ self.duration_MPC_planner = self.duration - self._idx/self._freq
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = self.q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = self.q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = self.ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ print(rospy.get_time() - time_begin)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+# def read_right_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+# def read_left_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+ def read_right_ee_grasp_ft_local_data_cb(self, msg):
+ self.F_ext_local_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5]])
+
+ def read_left_ee_grasp_ft_local_data_cb(self, msg):
+ self.F_ext_local_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5] ])
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment.py
new file mode 100755
index 0000000..b92ece8
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_experiment.py
@@ -0,0 +1,937 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 40)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ self.tf_listener = tf.TransformListener()
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+# self.dt_MPC_planner = 0.1 # time step
+ self.T_MPC_planner = 13 # T is number of time steps
+# self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_Right = builder_wholebodyMPC_planner.add_decision_variables('R_Right', 3, self.T_MPC_planner)
+ R_Left = builder_wholebodyMPC_planner.add_decision_variables('R_Left', 3, self.T_MPC_planner)
+# R_middle = builder_wholebodyMPC_planner.add_decision_variables('R_middle', 3, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+ r_ep_start = builder_wholebodyMPC_planner.add_decision_variables('r_ep_start', 12)
+
+ duration_MPC_planner = builder_wholebodyMPC_planner.add_parameter('duration_MPC_planner', 1)
+
+ t = np.linspace(0., 1., self.T_MPC_planner)
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+ # Add parameters
+# init_r_position_middle = builder_wholebodyMPC_planner.add_parameter('init_r_position_middle', 2)
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 3)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 3)
+
+ init_dr_position_Right = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Right', 3)
+ init_dr_position_Left = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Left', 3)
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+# r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Right[:, j]
+ r_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Left[:, j]
+ r_middle_var_MPC[:, i] = 0.5*(r_RARM_var_MPC[:, i]+r_LARM_var_MPC[:, i])
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_z' + str(i), optas.sumsqr(r_RARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_z' + str(i), optas.sumsqr(r_LARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_equality_constraint('middle' +str(i), r_middle_var_MPC[:, i], rhs=0.5*(r_RARM_var_MPC[:, i]+r_LARM_var_MPC[:, i]))
+
+
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - r_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_RARM_var_MPC[0, i], rhs=r_LARM_var_MPC[0, i])
+
+ # optimization cost: close to target
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_x', optas.sumsqr(r_middle_var_MPC[0, -1] - 0.5*(pos_R[0] + pos_L[0])))
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_y', 10*optas.sumsqr(r_middle_var_MPC[1, -1] - 0.5*(pos_R[1] + pos_L[1])))
+# builder_wholebodyMPC_planner.add_equality_constraint('Final_middle', r_middle_var_MPC[:, self.T_MPC_planner-1], rhs=0.5*(pos_R+pos_L))
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_arm', r_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_arm', r_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_L)
+
+
+
+
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+# builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2)
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', 10*optas.sumsqr(r_ep))
+
+ # add position constraint at the beginning state
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[:, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Right', R_Right[:, 0], rhs=init_r_position_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Left', R_Left[:, 0], rhs=init_r_position_Left)
+
+ for i in range(self.T_MPC_planner):
+ if(i<(self.T_MPC_planner -1)):
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance_x' + str(i), 50 * optas.sumsqr(R_Right[0, i+1] - R_Right[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance_y' + str(i), 50 * optas.sumsqr(R_Right[1, i+1] - R_Right[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance_z' + str(i), 50 * optas.sumsqr(R_Right[2, i+1] - R_Right[2, i]))
+
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance_x' + str(i), 50 * optas.sumsqr(R_Left[0, i+1] - R_Left[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance_y' + str(i), 50 * optas.sumsqr(R_Left[1, i+1] - R_Left[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance_z' + str(i), 50 * optas.sumsqr(R_Left[2, i+1] - R_Left[2, i]))
+
+ if(i<(self.T_MPC_planner -2)):
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance_x' + str(i), 50 * optas.sumsqr(R_Right[0, i+2]-2*R_Right[0, i+1] + R_Right[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance_y' + str(i), 50 * optas.sumsqr(R_Right[1, i+2]-2*R_Right[1, i+1] + R_Right[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance_z' + str(i), 50 * optas.sumsqr(R_Right[2, i+2]-2*R_Right[2, i+1] + R_Right[2, i]))
+
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_x' + str(i), 50 * optas.sumsqr(R_Left[0, i+2]-2*R_Left[0, i+1] + R_Left[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_y' + str(i), 50 * optas.sumsqr(R_Left[1, i+2]-2*R_Left[1, i+1] + R_Left[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_z' + str(i), 50 * optas.sumsqr(R_Left[2, i+2]-2*R_Left[2, i+1] + R_Left[2, i]))
+
+
+# dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ w_dr = duration_MPC_planner**2 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+# dr_middle_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+ dr_RARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Right[:, j+1] - R_Right[:, j])
+ dr_LARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Left[:, j+1] - R_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_right' + str(i), w_dr * optas.sumsqr(dr_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_left' + str(i), w_dr * optas.sumsqr(dr_LARM_var_MPC[:, i]))
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_middle', dr_middle_var_MPC[:, 0], rhs=0.5*(init_dr_position_Right + init_dr_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Right', dr_RARM_var_MPC[0:2, 0], rhs=init_dr_position_Right[0:2])
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Left', dr_LARM_var_MPC[0:2, 0], rhs=init_dr_position_Left[0:2])
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dr_middle', dr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_right', dr_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_left', dr_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+
+ lower_velocity = np.array([-0.9, -0.9, -0.5]); upper_velocity = np.array([0.9, 0.9, 0.5]);
+ for i in range(self.T_MPC_planner-1):
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity-r_ep_start[0:3], mid=(1./duration_MPC_planner) * self.n_planner * (R_Right[:, i+1] - R_Right[:, i]), rhs=upper_velocity+r_ep_start[0:3])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity-r_ep_start[3:6], mid=(1./duration_MPC_planner) * self.n_planner * (R_Left[:, i+1] - R_Left[:, i]), rhs=upper_velocity+r_ep_start[3:6])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_Right[:, i+1] - R_Right[:, i]), rhs=upper_velocity)
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_Left[:, i+1] - R_Left[:, i]), rhs=upper_velocity)
+
+# ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ w_ddr = duration_MPC_planner**4 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+# ddr_middle_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+ ddr_RARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Right[:, j+2] - 2*R_Right[:, j+1] + R_Right[:, j])
+ ddr_LARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Left[:, j+2] - 2*R_Left[:, j+1] + R_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_right' + str(i), w_ddr * optas.sumsqr(ddr_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_left' + str(i), w_ddr * optas.sumsqr(ddr_LARM_var_MPC[:, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_middle', ddr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_right', ddr_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_left', ddr_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+
+ lower_acceleration = np.array([-1., -1., -0.5]); upper_acceleration = np.array([1., 1., 0.5]);
+ for i in range(self.T_MPC_planner-2):
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration-r_ep_start[6:9], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Right[:, i+2] - 2 * R_Right[:, i+1] + R_Right[:, i]), rhs=upper_acceleration+r_ep_start[6:9])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration-r_ep_start[9:12], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Left[:, i+2] - 2 * R_Left[:, i+1] + R_Left[:, i]), rhs=upper_acceleration+r_ep_start[9:12])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Right[:, i+2] - 2 * R_Right[:, i+1] + R_Right[:, i]), rhs=upper_acceleration)
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Left[:, i+2] - 2 * R_Left[:, i+1] + R_Left[:, i]), rhs=upper_acceleration)
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right1', R_Right[:,8], rhs=R_Right[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right2', R_Right[:,9], rhs=R_Right[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left1', R_Left[:,8], rhs=R_Left[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left2', R_Left[:,9], rhs=R_Left[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_right', R_Right[:,self.T_MPC_planner-4], rhs=R_Right[:,self.T_MPC_planner-1])
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_left', R_Left[:,self.T_MPC_planner-4], rhs=R_Left[:,self.T_MPC_planner-1])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep_start', 10*optas.sumsqr(r_ep_start))
+
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.act_qpalg': 1,
+# 'knitro.FeasTol': 5e-4, 'knitro.OptTol': 5e-4, 'knitro.ftol':5e-4,
+ 'knitro.algorithm':3, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+
+ ### ---------------------------------------------------------
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 3000;
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+ #####################################################################################3
+ # get end-effector pose as parameters
+# pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+# ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+# pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+# ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+
+# pos_R_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+# pos_L_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ pos_R_reasonal = builder_wholebodyMPC.add_parameter('pos_R_reasonal', 3, self.T_MPC)
+ pos_L_reasonal = builder_wholebodyMPC.add_parameter('pos_L_reasonal', 3, self.T_MPC)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual_local = builder_wholebodyMPC.add_parameter('F_ext_Right_actual_local', 3)
+ F_ext_Left_actual_local = builder_wholebodyMPC.add_parameter('F_ext_Left_actual_local', 3)
+# F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+# F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ self.pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ self.ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.rotation_fnc_Right = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rotation_fnc_Left = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual_local + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual_local + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+# F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+# F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Two_arm orientation parallel' + str(i), 5*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i]).T @ self.ori_fnc_Left(q_var_MPC[:, i])))
+
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), 2*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), 2*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i]))
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Right(init_position_MPC) @ (F_ext_Right_var_MPC[:, i] - F_ext_Right_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(self.rotation_fnc_Left(init_position_MPC) @ (F_ext_Left_var_MPC[:, i] - F_ext_Left_goal[:, i]) - 0.5*m_box * ddpos_box_goal[:, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr( (self.rotation_fnc_Right(init_position_MPC) @ F_ext_Right_var_MPC[:, i] + self.rotation_fnc_Left(init_position_MPC) @ F_ext_Left_var_MPC[:, i]) - m_box * ddpos_box_goal[:, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), 10*optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+# builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[1, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[1, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[2, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[2, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = self.duration_MPC**2 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = self.duration_MPC**4 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+
+ #########################################################################################
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0,
+ 'print_time': 0,
+# 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5, 'knitro.ftol':1e-5,
+ 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4,
+ 'knitro.bar_penaltycons': 1, 'knitro.bar_penaltyrule':2,
+ 'knitro.bar_switchrule':2, 'knitro.linesearch': 1})
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+# self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+# self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_local_right = rospy.Subscriber("/chonk/sensor_ft_local_right", Float64MultiArray, self.read_right_ee_grasp_ft_local_data_cb)
+ self._sensor_ft_sub_local_left = rospy.Subscriber("/chonk/sensor_ft_local_left", Float64MultiArray, self.read_left_ee_grasp_ft_local_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ self.pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ self.pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ self.ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ self.ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, self.pos_Right[0], self.pos_Right[1], self.pos_Right[2], self.ori_Right[0], self.ori_Right[1], self.ori_Right[2], self.ori_Right[3],
+ self.pos_Left[0], self.pos_Left[1], self.pos_Left[2], self.ori_Left[0], self.ori_Left[1], self.ori_Left[2], self.ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self.duration_MPC_planner = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = self.pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = self.ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = self.pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = self.ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ # main execution
+ if(self._idx < self._steps):
+ time_begin = rospy.get_time()
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_actual_Right = np.array(self.pos_fnc_Right(self.q_curr))[0:3]
+ r_actual_Left = np.array(self.pos_fnc_Left(self.q_curr))[0:3]
+
+ dr_actual_Right = np.asarray(self.pos_Jac_fnc_Right(self.q_curr)) @ self.dq_curr
+ dr_actual_Left = np.asarray(self.pos_Jac_fnc_Left(self.q_curr)) @ self.dq_curr
+
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC planner
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ pos_R_goal = []; pos_L_goal = [];
+ _t = np.asarray(np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner))
+ for i in range(self.T_MPC_planner):
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': pos_R_goal,
+ f'R_Left': pos_L_goal, f'r_ep': np.zeros(self.T_MPC_planner),
+# f'r_ep_start': np.zeros(12)
+ })
+# self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': np.zeros((3, self.T_MPC_planner)),
+# f'R_Left': np.zeros((3, self.T_MPC_planner)), f'r_ep': np.zeros(self.T_MPC_planner) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': self.solution_MPC_planner[f'R_Right'],
+ f'R_Left': self.solution_MPC_planner[f'R_Left'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'],
+# f'r_ep_start': self.solution_MPC_planner[f'r_ep_start']
+ })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': self.pos_Right, 'pos_L': self.pos_Left,
+ 'init_r_position_Right': r_actual_Right,
+ 'init_r_position_Left': r_actual_Left,
+ 'init_dr_position_Right': dr_actual_Right,
+ 'init_dr_position_Left': dr_actual_Left,
+ 'duration_MPC_planner': self.duration_MPC_planner} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_Right = np.asarray(self.solution_MPC_planner[f'R_Right'])
+ R_Left = np.asarray(self.solution_MPC_planner[f'R_Left'])
+
+ pos_R_reasonal = np.zeros((3, self.T_MPC))
+ pos_L_reasonal = np.zeros((3, self.T_MPC))
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if((self.ti_MPC - self._t[self._idx-1]) <= self.duration_MPC_planner and self.duration_MPC_planner>= 2./self._freq):
+ t_nomalized = (self.ti_MPC - self._t[self._idx-1])/self.duration_MPC_planner
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Left[:, j]
+ else:
+ t_nomalized = 1.
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Left[:, j]
+# print(self._t[self._idx-1])
+# print(self.duration_MPC_planner)
+# print(R_Right[2, :])
+# print(pos_R_reasonal[2, :])
+# print('left')
+# print(R_Left[2, :])
+# print(pos_L_reasonal[2, :])
+ ### -----------------------------------------------------------
+ self.ti_MPC = 0
+ force_R_goal = []
+ force_L_goal = []
+ ori_R_goal = []
+ ori_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ self.G_Rotation_ee_right = self.rotation_fnc_Right(self.q_curr)
+ self.G_Rotation_ee_left = self.rotation_fnc_Left(self.q_curr)
+
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R_reasonal': pos_R_reasonal, 'pos_L_reasonal': pos_L_reasonal,
+ 'ori_R_reasonal': ori_R_goal, 'ori_L_reasonal': ori_L_goal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'inertia_Right': self.G_I_ee_r_conventional[3:6, 3:6], 'inertia_Left': self.G_I_ee_l_conventional[3:6, 3:6],
+ 'F_ext_Right_actual_local': self.F_ext_local_Right[3:6], 'F_ext_Left_actual_local': self.F_ext_local_Left[3:6],
+# 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_Left_actual': self.F_ext_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box } )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ self.q_next = np.zeros(self.ndof); p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ self.q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += (1./self.duration_MPC) * self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ self.dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ self.dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+
+ self.ddq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-2):
+ self.ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[1] = np.asarray(self.pos_fnc_Right(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[1] = np.asarray(self.pos_fnc_Left(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0][1]
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., self.dq_next[2]])
+ Global_v_b = np.asarray([self.dq_next[0], self.dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+ self.duration_MPC_planner = self.duration - self._idx/self._freq
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = self.q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = self.q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = self.ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ print(rospy.get_time() - time_begin)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+# def read_right_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+# def read_left_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+ def read_right_ee_grasp_ft_local_data_cb(self, msg):
+ self.F_ext_local_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5]])
+
+ def read_left_ee_grasp_ft_local_data_cb(self, msg):
+ self.F_ext_local_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5] ])
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py
new file mode 100755
index 0000000..be74908
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_obstacle_wholetrajectory_withOrientation.py
@@ -0,0 +1,1051 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 40)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+# self.dt_MPC_planner = 0.1 # time step
+ self.T_MPC_planner = 10 # T is number of time steps
+# self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_pos_Right = builder_wholebodyMPC_planner.add_decision_variables('R_pos_Right', 3, self.T_MPC_planner)
+ R_pos_Left = builder_wholebodyMPC_planner.add_decision_variables('R_pos_Left', 3, self.T_MPC_planner)
+ R_ori_Right = builder_wholebodyMPC_planner.add_decision_variables('R_ori_Right', 4, self.T_MPC_planner)
+ R_ori_Left = builder_wholebodyMPC_planner.add_decision_variables('R_ori_Left', 4, self.T_MPC_planner)
+# R_middle = builder_wholebodyMPC_planner.add_decision_variables('R_middle', 3, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+# r_ep_start = builder_wholebodyMPC_planner.add_decision_variables('r_ep_start', 12)
+# r_ori_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ori_ep', 2, self.T_MPC_planner)
+
+
+ duration_MPC_planner = builder_wholebodyMPC_planner.add_parameter('duration_MPC_planner', 1)
+
+ self.pos_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_position_function(link=self._link_ee_left)
+ self.ori_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_quaternion_function(link=self._link_ee_left)
+
+ self.pos_Jac_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ self.ori_Jac_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_right)
+ self.ori_Jac_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_left)
+
+ t = np.linspace(0., 1., self.T_MPC_planner)
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+ # Add parameters
+# init_r_position_middle = builder_wholebodyMPC_planner.add_parameter('init_r_position_middle', 2)
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 3)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 3)
+ init_r_orientation_Right = builder_wholebodyMPC_planner.add_parameter('init_r_orientation_Right', 4)
+ init_r_orientation_Left = builder_wholebodyMPC_planner.add_parameter('init_r_orientation_Left', 4)
+
+ init_dr_position_Right = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Right', 3)
+ init_dr_position_Left = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Left', 3)
+ init_dr_orientation_Right = builder_wholebodyMPC_planner.add_parameter('init_dr_orientation_Right', 4)
+ init_dr_orientation_Left = builder_wholebodyMPC_planner.add_parameter('init_dr_orientation_Left', 4)
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3)
+ ori_R = builder_wholebodyMPC_planner.add_parameter('ori_R', 4)
+ ori_L = builder_wholebodyMPC_planner.add_parameter('ori_L', 4)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ r_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+# r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_pos_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_pos_Right[:, j]
+ r_pos_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_pos_Left[:, j]
+ r_ori_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_ori_Right[:, j]
+ r_ori_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_ori_Left[:, j]
+ r_middle_var_MPC[:, i] = 0.5*(r_pos_RARM_var_MPC[:, i]+r_pos_LARM_var_MPC[:, i])
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_z' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_z' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_equality_constraint('middle' +str(i), r_middle_var_MPC[:, i], rhs=0.5*(r_pos_RARM_var_MPC[:, i]+r_pos_LARM_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_leq_inequality_constraint('quaternion_equality_right' + str(i), lhs=optas.sumsqr(R_ori_Right[:, i]), rhs=1. + r_ori_ep[0, i])
+# builder_wholebodyMPC_planner.add_leq_inequality_constraint('quaternion_equality_left' + str(i), lhs=optas.sumsqr(R_ori_Left[:, i]), rhs=1. + r_ori_ep[1, i])
+ builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_right' + str(i), lhs=optas.sumsqr(R_ori_Right[:, i]), rhs=1.)
+ builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_left' + str(i), lhs=optas.sumsqr(R_ori_Left[:, i]), rhs=1.)
+ builder_wholebodyMPC_planner.add_cost_term('Two_arm orientation parallel' + str(i), optas.sumsqr(R_ori_Right[:, i].T @ R_ori_Left[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Two_arm end height same' + str(i), optas.sumsqr(R_pos_Right[2, i] - R_pos_Left[2, i]))
+
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[0, i] - r_pos_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_pos_RARM_var_MPC[0, i], rhs=r_pos_LARM_var_MPC[0, i])
+
+ # optimization cost: close to target
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_x', optas.sumsqr(r_middle_var_MPC[0, -1] - 0.5*(pos_R[0] + pos_L[0])))
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_y', 10*optas.sumsqr(r_middle_var_MPC[1, -1] - 0.5*(pos_R[1] + pos_L[1])))
+# builder_wholebodyMPC_planner.add_equality_constraint('Final_middle', r_middle_var_MPC[:, self.T_MPC_planner-1], rhs=0.5*(pos_R+pos_L))
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_arm', r_pos_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_arm', r_pos_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_L)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_ori_arm', r_ori_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=ori_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_ori_arm', r_ori_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=ori_L)
+
+
+
+
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.3 + 1
+# builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2)
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', 10*optas.sumsqr(r_ep))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_r_ori_ep', 10*optas.sumsqr(r_ori_ep))
+
+ # add position constraint at the beginning state
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[:, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Right', R_pos_Right[:, 0], rhs=init_r_position_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Left', R_pos_Left[:, 0], rhs=init_r_position_Left)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_ori_Right', R_ori_Right[:, 0], rhs=init_r_orientation_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_ori_Left', R_ori_Left[:, 0], rhs=init_r_orientation_Left)
+
+ for i in range(self.T_MPC_planner):
+ if(i<(self.T_MPC_planner -1)):
+# builder_wholebodyMPC_planner.add_cost_term('Right_distance' + str(i), 50 * optas.sumsqr(R_pos_Right[:, i+1] - R_pos_Right[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_distance' + str(i), 50 * optas.sumsqr(R_pos_Left[:, i+1] - R_pos_Left[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance_x' + str(i), 50 * optas.sumsqr(R_pos_Right[0, i+1] - R_pos_Right[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance_y' + str(i), 50 * optas.sumsqr(R_pos_Right[1, i+1] - R_pos_Right[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance_z' + str(i), 100 * optas.sumsqr(R_pos_Right[2, i+1] - R_pos_Right[2, i]))
+
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance_x' + str(i), 50 * optas.sumsqr(R_pos_Left[0, i+1] - R_pos_Left[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance_y' + str(i), 50 * optas.sumsqr(R_pos_Left[1, i+1] - R_pos_Left[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance_z' + str(i), 100 * optas.sumsqr(R_pos_Left[2, i+1] - R_pos_Left[2, i]))
+
+ builder_wholebodyMPC_planner.add_cost_term('Right_ori_distance' + str(i), 20 * optas.sumsqr(R_ori_Right[:, i+1] - R_ori_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_ori_distance' + str(i), 20 * optas.sumsqr(R_ori_Left[:, i+1] - R_ori_Left[:, i]))
+ if(i<(self.T_MPC_planner -2)):
+# builder_wholebodyMPC_planner.add_cost_term('dRight_distance' + str(i), 50 * optas.sumsqr(R_pos_Right[:, i+2]-2*R_pos_Right[:, i+1] + R_pos_Right[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('dLeft_distance' + str(i), 50 * optas.sumsqr(R_pos_Left[:, i+2] -2*R_pos_Left[:, i+1] + R_pos_Left[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance_x' + str(i), 50 * optas.sumsqr(R_pos_Right[0, i+2]-2*R_pos_Right[0, i+1] + R_pos_Right[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance_y' + str(i), 50 * optas.sumsqr(R_pos_Right[1, i+2]-2*R_pos_Right[1, i+1] + R_pos_Right[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance_z' + str(i), 100 * optas.sumsqr(R_pos_Right[2, i+2]-2*R_pos_Right[2, i+1] + R_pos_Right[2, i]))
+
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_x' + str(i), 50 * optas.sumsqr(R_pos_Left[0, i+2]-2*R_pos_Left[0, i+1] + R_pos_Left[0, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_y' + str(i), 50 * optas.sumsqr(R_pos_Left[1, i+2]-2*R_pos_Left[1, i+1] + R_pos_Left[1, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_z' + str(i), 100 * optas.sumsqr(R_pos_Left[2, i+2]-2*R_pos_Left[2, i+1] + R_pos_Left[2, i]))
+
+ builder_wholebodyMPC_planner.add_cost_term('dRight_ori_distance' + str(i), 20 * optas.sumsqr(R_ori_Right[:, i+2]-2*R_ori_Right[:, i+1] + R_ori_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_ori_distance' + str(i), 20 * optas.sumsqr(R_ori_Left[:, i+2] -2*R_ori_Left[:, i+1] + R_ori_Left[:, i]))
+
+# dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ dr_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+ w_dr = duration_MPC_planner**2 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+# dr_middle_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+ dr_pos_RARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_pos_Right[:, j+1] - R_pos_Right[:, j])
+ dr_pos_LARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_pos_Left[:, j+1] - R_pos_Left[:, j])
+ dr_ori_RARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_ori_Right[:, j+1] - R_ori_Right[:, j])
+ dr_ori_LARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_ori_Left[:, j+1] - R_ori_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_right' + str(i), w_dr * optas.sumsqr(dr_pos_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_left' + str(i), w_dr * optas.sumsqr(dr_pos_LARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_ori_right' + str(i), w_dr * optas.sumsqr(dr_ori_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_ori_left' + str(i), w_dr * optas.sumsqr(dr_ori_LARM_var_MPC[:, i]))
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_middle', dr_middle_var_MPC[:, 0], rhs=0.5*(init_dr_position_Right + init_dr_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Right', dr_pos_RARM_var_MPC[0:2, 0], rhs=init_dr_position_Right[0:2])
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Left', dr_pos_LARM_var_MPC[0:2, 0], rhs=init_dr_position_Left[0:2])
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Right', dr_ori_RARM_var_MPC[:, 0], rhs=init_dr_orientation_Right)
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Left', dr_ori_LARM_var_MPC[:, 0], rhs=init_dr_orientation_Left)
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dr_middle', dr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_right', dr_pos_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_left', dr_pos_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_ori_right', dr_ori_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_ori_left', dr_ori_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+
+ lower_velocity = np.array([-0.9, -0.9, -0.5]); upper_velocity = np.array([0.9, 0.9, 0.5]);
+ for i in range(self.T_MPC_planner-1):
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity-r_ep_start[0:3], mid=(1./duration_MPC_planner) * self.n_planner * (R_pos_Right[:, i+1] - R_pos_Right[:, i]), rhs=upper_velocity+r_ep_start[0:3])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity-r_ep_start[3:6], mid=(1./duration_MPC_planner) * self.n_planner * (R_pos_Left[:, i+1] - R_pos_Left[:, i]), rhs=upper_velocity+r_ep_start[3:6])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_pos_Right[:, i+1] - R_pos_Right[:, i]), rhs=upper_velocity)
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_pos_Left[:, i+1] - R_pos_Left[:, i]), rhs=upper_velocity)
+
+# ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ ddr_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+ w_ddr = duration_MPC_planner**4 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+# ddr_middle_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+ ddr_pos_RARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_pos_Right[:, j+2] - 2*R_pos_Right[:, j+1] + R_pos_Right[:, j])
+ ddr_pos_LARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_pos_Left[:, j+2] - 2*R_pos_Left[:, j+1] + R_pos_Left[:, j])
+ ddr_ori_RARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_ori_Right[:, j+2] - 2*R_ori_Right[:, j+1] + R_ori_Right[:, j])
+ ddr_ori_LARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_ori_Left[:, j+2] - 2*R_ori_Left[:, j+1] + R_ori_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_right' + str(i), w_ddr * optas.sumsqr(ddr_pos_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_left' + str(i), w_ddr * optas.sumsqr(ddr_pos_LARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_ori_right' + str(i), w_ddr * optas.sumsqr(ddr_ori_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_ori_left' + str(i), w_ddr * optas.sumsqr(ddr_ori_LARM_var_MPC[:, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_two arm equal' + str(i), w_ddr * 10 * optas.sumsqr(ddr_pos_RARM_var_MPC[:, i] - ddr_pos_LARM_var_MPC[:, i]))
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_middle', ddr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_right', ddr_pos_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_left', ddr_pos_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_ori_right', ddr_ori_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_ori_left', ddr_ori_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+
+ lower_acceleration = np.array([-1., -1., -0.5]); upper_acceleration = np.array([1., 1., 0.5]);
+ for i in range(self.T_MPC_planner-2):
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration-r_ep_start[6:9], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_pos_Right[:, i+2] - 2 * R_pos_Right[:, i+1] + R_pos_Right[:, i]), rhs=upper_acceleration+r_ep_start[6:9])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration-r_ep_start[9:12], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_pos_Left[:, i+2] - 2 * R_pos_Left[:, i+1] + R_pos_Left[:, i]), rhs=upper_acceleration+r_ep_start[9:12])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_pos_Right[:, i+2] - 2 * R_pos_Right[:, i+1] + R_pos_Right[:, i]), rhs=upper_acceleration)
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_pos_Left[:, i+2] - 2 * R_pos_Left[:, i+1] + R_pos_Left[:, i]), rhs=upper_acceleration)
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right1', R_pos_Right[:,8], rhs=R_pos_Right[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right2', R_pos_Right[:,9], rhs=R_pos_Right[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left1', R_pos_Left[:,8], rhs=R_pos_Left[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left2', R_pos_Left[:,9], rhs=R_pos_Left[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_right', R_pos_Right[:,self.T_MPC_planner-4], rhs=R_pos_Right[:,self.T_MPC_planner-1])
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_left', R_pos_Left[:,self.T_MPC_planner-4], rhs=R_pos_Left[:,self.T_MPC_planner-1])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep_start', 100*optas.sumsqr(r_ep_start))
+
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0,
+ 'print_time': 0,
+# 'knitro.par_msnumthreads': 14,
+ 'knitro.act_qpalg': 1,
+ 'knitro.FeasTol': 5e-4, 'knitro.OptTol': 5e-4, 'knitro.ftol':5e-4,
+ 'knitro.algorithm':3, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+
+ ### ---------------------------------------------------------
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 1500;
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+ #####################################################################################3
+ # get end-effector pose as parameters
+# pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+# ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+# pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+# ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+
+# pos_R_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+# pos_L_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ pos_R_reasonal = builder_wholebodyMPC.add_parameter('pos_R_reasonal', 3, self.T_MPC)
+ pos_L_reasonal = builder_wholebodyMPC.add_parameter('pos_L_reasonal', 3, self.T_MPC)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual_local = builder_wholebodyMPC.add_parameter('F_ext_Right_actual_local', 3)
+ F_ext_Left_actual_local = builder_wholebodyMPC.add_parameter('F_ext_Left_actual_local', 3)
+# F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+# F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ self.pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_fnc_Right_Base = self.wholebodyMPC.get_link_position_function(link=self._link_ee_right, base_link = self._link_donkey)
+ self.pos_fnc_Left_Base = self.wholebodyMPC.get_link_position_function(link=self._link_ee_left, base_link = self._link_donkey)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ self.ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.ori_fnc_donkey = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_donkey)
+ self.rotation_fnc_Right = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rotation_fnc_Left = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual_local + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual_local + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+# F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+# F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Two_arm orientation parallel' + str(i), 0.1*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i]).T @ self.ori_fnc_Left(q_var_MPC[:, i])))
+# quaternion_donkey_start = optas.spatialmath.Quaternion.fromrpy([np.pi/2, 0, 0]).getquat()
+# builder_wholebodyMPC.add_cost_term('Two_arm orientation parallel with donkey plane' + str(i), 10*optas.sumsqr(self.ori_fnc_donkey(q_var_MPC[:, i]).__mul__(quaternion_donkey_start).T @ self.ori_fnc_Right(q_var_MPC[:, i]) ))
+
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - self.rotation_fnc_Right(init_position_MPC) @ Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - self.rotation_fnc_Left(init_position_MPC) @ Delta_p_Left_var_MPC[:, i]))
+
+# builder_wholebodyMPC.add_cost_term('Two_arm position forward relative to floatingbase x' + str(i), 5*optas.sumsqr(self.pos_fnc_Right_Base(q_var_MPC[:, i])[0]- self.pos_fnc_Left_Base(q_var_MPC[:, i])[0]))
+# builder_wholebodyMPC.add_cost_term('Two_arm position forward relative to floatingbase z' + str(i), 5*optas.sumsqr(self.pos_fnc_Right_Base(q_var_MPC[:, i])[2]- self.pos_fnc_Left_Base(q_var_MPC[:, i])[2]))
+
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr((self.rotation_fnc_Right(init_position_MPC) @ (F_ext_Right_var_MPC[:, i] - F_ext_Right_goal[:, i]))[0:2] - 0.5*m_box * ddpos_box_goal[0:2, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr((self.rotation_fnc_Left(init_position_MPC) @ (F_ext_Left_var_MPC[:, i] - F_ext_Left_goal[:, i]))[0:2] - 0.5*m_box * ddpos_box_goal[0:2, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr( ((self.rotation_fnc_Right(init_position_MPC) @ F_ext_Right_var_MPC[:, i] + self.rotation_fnc_Left(init_position_MPC) @ F_ext_Left_var_MPC[:, i]))[0:2] - m_box * ddpos_box_goal[0:2, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), 10* optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+# builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[1, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[1, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[2, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[2, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = self.duration_MPC**2 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = self.duration_MPC**4 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+ acc_box_var[:, self.T_MPC-1] = acc_box_var[:, self.T_MPC-2]
+ #########################################################################################
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5, 'knitro.ftol':1e-5,
+ 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4,
+ 'knitro.bar_penaltycons': 1, 'knitro.bar_penaltyrule':2,
+ 'knitro.bar_switchrule':2, 'knitro.linesearch': 1})
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+# self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+# self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_local_right = rospy.Subscriber("/chonk/sensor_ft_local_right", Float64MultiArray, self.read_right_ee_grasp_ft_local_data_cb)
+ self._sensor_ft_sub_local_left = rospy.Subscriber("/chonk/sensor_ft_local_left", Float64MultiArray, self.read_left_ee_grasp_ft_local_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ self.pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ self.pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ self.ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ self.ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, self.pos_Right[0], self.pos_Right[1], self.pos_Right[2], self.ori_Right[0], self.ori_Right[1], self.ori_Right[2], self.ori_Right[3],
+ self.pos_Left[0], self.pos_Left[1], self.pos_Left[2], self.ori_Left[0], self.ori_Left[1], self.ori_Left[2], self.ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self.duration_MPC_planner = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = self.pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = self.ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = self.pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = self.ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_pos_actual_Right = np.array(self.pos_fnc_Right_planner(self.q_curr))[0:3]
+ r_pos_actual_Left = np.array(self.pos_fnc_Left_planner(self.q_curr))[0:3]
+ dr_pos_actual_Right = np.asarray(self.pos_Jac_fnc_Right_planner(self.q_curr)) @ self.dq_curr
+ dr_pos_actual_Left = np.asarray(self.pos_Jac_fnc_Left_planner(self.q_curr)) @ self.dq_curr
+
+ r_ori_actual_Right = np.array(self.ori_fnc_Right_planner(self.q_curr))[0:4]
+ r_ori_actual_Left = np.array(self.ori_fnc_Left_planner(self.q_curr))[0:4]
+ dr_ori_actual_Right = self.angular_velocity_to_quaternionRate(r_ori_actual_Right[:, 0]) @ np.asarray(self.ori_Jac_fnc_Right_planner(self.q_curr)) @ self.dq_curr
+ dr_ori_actual_Left = self.angular_velocity_to_quaternionRate(r_ori_actual_Left[:, 0]) @ np.asarray(self.ori_Jac_fnc_Left_planner(self.q_curr)) @ self.dq_curr
+
+# print(r_ori_actual_Right)
+# print(r_ori_actual_Left)
+# print(r_ori_actual_Right.T @ r_ori_actual_Left)
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC planner
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ pos_R_goal = []; pos_L_goal = []; ori_R_goal = []; ori_L_goal = [];
+ _t = np.asarray(np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner))
+ for i in range(self.T_MPC_planner):
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(_t[i]).flatten()
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(_t[i]).flatten()
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_pos_Right': pos_R_goal, f'R_pos_Left': pos_L_goal,
+ f'r_ep': np.zeros(self.T_MPC_planner),
+# f'r_ep_start': np.zeros(12),
+ f'R_ori_Right': ori_R_goal, f'R_ori_Left': ori_L_goal,
+# f'r_ori_ep': np.zeros((2, self.T_MPC_planner)),
+ })
+# self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_pos_Right': np.zeros((3, self.T_MPC_planner)),
+# f'R_pos_Left': np.zeros((3, self.T_MPC_planner)), f'r_ep': np.zeros(self.T_MPC_planner) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_pos_Right': self.solution_MPC_planner[f'R_pos_Right'],
+ f'R_pos_Left': self.solution_MPC_planner[f'R_pos_Left'],
+ f'R_ori_Right': self.solution_MPC_planner[f'R_ori_Right'],
+ f'R_ori_Left': self.solution_MPC_planner[f'R_ori_Left'],
+# f'r_ori_ep': self.solution_MPC_planner[f'r_ori_ep'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'],
+# f'r_ep_start': self.solution_MPC_planner[f'r_ep_start']
+ })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': self.pos_Right, 'pos_L': self.pos_Left,
+ 'ori_R': self.ori_Right, 'ori_L': self.ori_Left,
+ 'init_r_position_Right': r_pos_actual_Right, 'init_r_position_Left': r_pos_actual_Left,
+ 'init_dr_position_Right': dr_pos_actual_Right, 'init_dr_position_Left': dr_pos_actual_Left,
+ 'init_r_orientation_Right': r_ori_actual_Right, 'init_r_orientation_Left': r_ori_actual_Left,
+ 'init_dr_orientation_Right': dr_ori_actual_Right, 'init_dr_orientation_Left': dr_ori_actual_Left,
+ 'duration_MPC_planner': self.duration_MPC_planner} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_pos_Right = np.asarray(self.solution_MPC_planner[f'R_pos_Right'])
+ R_pos_Left = np.asarray(self.solution_MPC_planner[f'R_pos_Left'])
+ R_ori_Right = np.asarray(self.solution_MPC_planner[f'R_ori_Right'])
+ R_ori_Left = np.asarray(self.solution_MPC_planner[f'R_ori_Left'])
+
+ pos_R_reasonal = np.zeros((3, self.T_MPC))
+ pos_L_reasonal = np.zeros((3, self.T_MPC))
+ ori_R_reasonal = np.zeros((4, self.T_MPC))
+ ori_L_reasonal = np.zeros((4, self.T_MPC))
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+# if((self.ti_MPC - self._t[self._idx-1]) <= self.duration_MPC_planner and self.duration_MPC_planner>= 2./self._freq):
+ if((self.ti_MPC - self._t[self._idx-1]) <= self.duration_MPC_planner):
+ t_nomalized = (self.ti_MPC - self._t[self._idx-1])/self.duration_MPC_planner
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Left[:, j]
+ ori_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Right[:, j]
+ ori_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Left[:, j]
+ else:
+ t_nomalized = 1.
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Left[:, j]
+ ori_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Right[:, j]
+ ori_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Left[:, j]
+# print(self._t[self._idx-1])
+# print(self.duration_MPC_planner)
+# print(R_ori_Right)
+# print(pos_R_reasonal[2, :])
+# print('left')
+# print(R_pos_Left[2, :])
+# print(pos_L_reasonal[2, :])
+ ### -----------------------------------------------------------
+ self.ti_MPC = 0
+ force_R_goal = []
+ force_L_goal = []
+# ori_R_goal = []
+# ori_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+# g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# ori_R_goal.append(g_rarm_ee_ori.tolist())
+# g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+# ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+# ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+# ori_R_goal = optas.np.array(ori_R_goal).T
+# ori_L_goal = optas.np.array(ori_L_goal).T
+
+ # read current robot joint positions
+# self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+# self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ self.G_Rotation_ee_right = self.rotation_fnc_Right(self.q_curr)
+ self.G_Rotation_ee_left = self.rotation_fnc_Left(self.q_curr)
+
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R_reasonal': pos_R_reasonal, 'pos_L_reasonal': pos_L_reasonal,
+ 'ori_R_reasonal': ori_R_reasonal, 'ori_L_reasonal': ori_L_reasonal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'inertia_Right': self.G_I_ee_r_conventional[3:6, 3:6], 'inertia_Left': self.G_I_ee_l_conventional[3:6, 3:6],
+# 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_Left_actual': self.F_ext_Left[3:6],
+ 'F_ext_Right_actual_local': self.F_ext_local_Right[3:6], 'F_ext_Left_actual_local': self.F_ext_local_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box } )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ self.q_next = np.zeros(self.ndof); p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ self.q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += (1./self.duration_MPC) * self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ self.dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ self.dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+
+ self.ddq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-2):
+ self.ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[1] = np.asarray(self.pos_fnc_Right(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[1] = np.asarray(self.pos_fnc_Left(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0][1]
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., self.dq_next[2]])
+ Global_v_b = np.asarray([self.dq_next[0], self.dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+ self.duration_MPC_planner = self.duration - self._idx/self._freq
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = self.q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = self.q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = self.ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+# def read_right_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+# def read_left_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+ def read_right_ee_grasp_ft_local_data_cb(self, msg):
+ self.F_ext_local_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5]])
+
+ def read_left_ee_grasp_ft_local_data_cb(self, msg):
+ self.F_ext_local_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, 0, msg.data[5] ])
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+ def angular_velocity_to_quaternionRate(self, quaternion):
+ A = 0.5* np.asarray([ [ quaternion[3], quaternion[2], -quaternion[1] ],
+ [ -quaternion[2], quaternion[3], quaternion[0] ],
+ [ quaternion[1], -quaternion[0], quaternion[3] ],
+ [ -quaternion[0], -quaternion[1], -quaternion[2] ]
+ ])
+# A = optas.casadi.SX(np.zeros((4, 3)))
+# A[0,0] = quaternion[3]; A[0,1] = quaternion[2]; A[0,2] = -quaternion[1];
+# A[1,0] = -quaternion[2]; A[1,1] = quaternion[3]; A[1,2] = quaternion[0];
+# A[2,0] = quaternion[1]; A[2,1] = -quaternion[0]; A[2,2] = quaternion[3];
+# A[3,0] = -quaternion[0]; A[3,1] = -quaternion[1]; A[3,2] = -quaternion[2];
+ return A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD.py
new file mode 100755
index 0000000..ea8f9e1
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD.py
@@ -0,0 +1,713 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 3000;
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+ #####################################################################################3
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+ ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+ pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+ ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+ F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ self.pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ self.ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.rotation_fnc_Right = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rotation_fnc_Left = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), 2*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R[:, i] - init_Delta_position_Right - Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), 2*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L[:, i] - init_Delta_position_Left - Delta_p_Left_var_MPC[:, i]))
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Right_var_MPC[1, i] - F_ext_Right_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Left_var_MPC[1, i] - F_ext_Left_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[1, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[1, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0001/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ ddlower = np.array([-1.5, -1.5, -1.5]); ddupper = np.array([1.5, 1.5, 1.5]);
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ if(i<(self.T_MPC -2)):
+ name = 'control_point_dderiv_' + str(i) + '_bound' # add acceleration constraint for Q[0:3, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=ddlower, mid=(1./self.duration_MPC)**2 * self.n * (self.n - 1) * (Q[0:3, i+2]- 2*Q[0:3, i+1] + Q[0:3, i]), rhs=ddupper)
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+
+ #########################################################################################
+ # setup solver
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5,
+ 'knitro.ftol':1e-5, 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+ 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4,
+ 'knitro.bar_penaltycons': 1, 'knitro.bar_penaltyrule':2,
+ 'knitro.bar_switchrule':2, 'knitro.linesearch': 1})
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare ft_sensor subscriber
+# self._ft_right_sub = rospy.Subscriber("/ft_right/raw/data", WrenchStamped, self.read_ft_sensor_right_data_cb)
+# self._ft_left_sub = rospy.Subscriber("/ft_left/raw/data", WrenchStamped, self.read_ft_sensor_left_data_cb)
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+# self._joint_pub = rospy.Publisher(self._pub_cmd_topic_name, Float64MultiArray, queue_size=10)
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+ self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ # declare two arm ee grasp actual force publisher
+# self._acc_sub = rospy.Subscriber("/chonk/acc_box", Float64MultiArray, self.read_box_acc_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, pos_Right[0], pos_Right[1], pos_Right[2], ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2], ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ time = rospy.get_time()
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self.ti_MPC = 0 # time index of the MPC
+ self._idx += 1
+ pos_R_goal = []; ori_R_goal = []; pos_L_goal = []; ori_L_goal = []; force_R_goal = []; force_L_goal = [];
+# ddpos_box_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+# g_box_ddpos = self._RARM_ee_ddPos_trajectory(self.ti_MPC).flatten()
+# ddpos_box_goal.append(g_box_ddpos.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+# ddpos_box_goal.append(g_box_ddpos.tolist())
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+# ddpos_box_goal = optas.np.array(ddpos_box_goal).T
+
+# print(ddpos_box_goal)
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+# self.Derivation_RARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1] - pos_R_goal[1, 0]
+# self.Derivation_LARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1] - pos_L_goal[1, 0]
+
+ self.G_Rotation_ee_right = self.rotation_fnc_Right(self.q_curr)
+ self.G_Rotation_ee_left = self.rotation_fnc_Left(self.q_curr)
+
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+
+# print(self.G_I_ee_r_conventional[3:6, 3:6])
+# print(self.G_I_ee_l_conventional[3:6, 3:6])
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R': pos_R_goal, 'ori_R': ori_R_goal, 'pos_L': pos_L_goal, 'ori_L': ori_L_goal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'inertia_Right': self.G_I_ee_r_conventional[3:6, 3:6], 'inertia_Left': self.G_I_ee_l_conventional[3:6, 3:6],
+ 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_Left_actual': self.F_ext_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box} )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ q_next = np.zeros(self.ndof); p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += (1./self.duration_MPC) * self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+
+ ddq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-2):
+ ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+
+# self.acc_box = 0.5*(self.pos_Jac_fnc_Right(q_next) + self.pos_Jac_fnc_Left(q_next)) @ ddq_next
+# F_r = np.zeros(3)
+# F_l = np.zeros(3)
+# Delta_p_r = np.zeros(3)
+# dDelta_p_r = np.zeros(3)
+# ddDelta_p_r = np.zeros(3)
+# Delta_p_l = np.zeros(3)
+# dDelta_p_l = np.zeros(3)
+# ddDelta_p_l = np.zeros(3)
+# for j in range(self.T_MPC):
+# Delta_p_r += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+# Delta_p_l += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Left[:, j]
+# for j in range(self.T_MPC-1):
+# dDelta_p_r += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P_Right[:, j+1] - P_Right[:, j])
+# dDelta_p_l += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P_Left[:, j+1] - P_Left[:, j])
+# for j in range(self.T_MPC-2):
+# ddDelta_p_r += self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+# ddDelta_p_l += self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+# #####################################################################################
+# F_r = self.F_ext_Right[3:6] + self.K_Right @ Delta_p_r
+# F_l = self.F_ext_Left[3:6] + self.K_Left @ Delta_p_l
+# F_r = self.F_ext_Right[3:6] + self.G_I_ee_r_conventional[3:6, 3:6] @ ddDelta_p_r + self.K_Right @ Delta_p_r + self.D_Right @ dDelta_p_r
+# F_l = self.F_ext_Left[3:6] + self.G_I_ee_l_conventional[3:6, 3:6] @ ddDelta_p_l + self.K_Left @ Delta_p_l + self.D_Left @ dDelta_p_l
+
+# print(F_r)
+# print(F_l)
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[1] = np.asarray(self.pos_fnc_Right(q_next)).T[0][1] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[1] = np.asarray(self.pos_fnc_Left(q_next)).T[0][1] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0][1]
+
+# self.Derivation_RARM_pos_start[1] = pos_R_goal[1, 0] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1]
+# self.Derivation_LARM_pos_start[1] = pos_L_goal[1, 0] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1]
+
+# print(self.Derivation_RARM_pos_start)
+# print(self.Derivation_LARM_pos_start)
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ print(rospy.get_time()-time)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+# def read_ft_sensor_right_data_cb(self, msg):
+# self.ft_right = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+# def read_ft_sensor_left_data_cb(self, msg):
+# self.ft_left = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+ def read_right_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5] ])
+ self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+ def read_left_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5] ])
+ self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+# def read_box_acc_data_cb(self, msg):
+# self.acc_box = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.py
new file mode 100755
index 0000000..a68971c
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_longhorizon.py
@@ -0,0 +1,938 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+ self.dt_MPC_planner = 0.1 # time step
+ self.T_MPC_planner = 10 # T is number of time steps
+ self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_Right = builder_wholebodyMPC_planner.add_decision_variables('R_Right', 3, self.T_MPC_planner)
+ R_Left = builder_wholebodyMPC_planner.add_decision_variables('R_Left', 3, self.T_MPC_planner)
+ R_middle = builder_wholebodyMPC_planner.add_decision_variables('R_middle', 3, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+
+ t = builder_wholebodyMPC_planner.add_parameter('t', self.T_MPC_planner) # time
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+ # Add parameters
+# init_r_position_middle = builder_wholebodyMPC_planner.add_parameter('init_r_position_middle', 2)
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 2)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 2)
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3, self.T_MPC_planner)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3, self.T_MPC_planner)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+ r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Right[:, j]
+ r_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Left[:, j]
+ # optimization cost: close to target
+ builder_wholebodyMPC_planner.add_cost_term('Right_middle_x' + str(i), optas.sumsqr(r_middle_var_MPC[0, i] - 0.5*(pos_R[0, i] + pos_L[0, i])))
+ builder_wholebodyMPC_planner.add_cost_term('Right_middle_y' + str(i), 10*optas.sumsqr(r_middle_var_MPC[1, i] - 0.5*(pos_R[1, i] + pos_L[1, i])))
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - r_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_RARM_var_MPC[0, i], rhs=r_LARM_var_MPC[0, i])
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_middle_z', r_middle_var_MPC[2, :], rhs=0.5*(pos_R[2, :]+pos_L[2, :]))
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_xy', r_RARM_var_MPC[0:2, :], rhs=r_middle_var_MPC[0:2, :] - 0.5*(pos_L[0:2, :]-pos_R[0:2, :]))
+ builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_xy', r_LARM_var_MPC[0:2, :], rhs=r_middle_var_MPC[0:2, :] + 0.5*(pos_L[0:2, :]-pos_R[0:2, :]))
+
+ builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_z', r_RARM_var_MPC[2, :], rhs=r_middle_var_MPC[2, :])
+ builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_z', r_LARM_var_MPC[2, :], rhs=r_middle_var_MPC[2, :])
+# builder_wholebodyMPC_planner.add_equality_constraint('Right_arm_z', R_Right[2, :], rhs=pos_R[2, :])
+# builder_wholebodyMPC_planner.add_equality_constraint('Left_arm_z', R_Left[2, :], rhs=pos_L[2, :])
+
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', optas.sumsqr(r_ep))
+
+ # add position constraint at the beginning state
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[0:2, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+
+
+ dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ w_dr = 0.05/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+ dr_middle_var_MPC[:, i] += (1./self.duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+# dr_RARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Right[:, j+1] - R_Right[:, j])
+# dr_LARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Left[:, j+1] - R_Left[:, j])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_right' + str(i), w_dq * optas.sumsqr(dr_RARM_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_left' + str(i), w_dq * optas.sumsqr(dr_LARM_var_MPC[:, i]))
+
+ ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ w_ddr = 0.05/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+ ddr_middle_var_MPC[:, i] += (1./self.duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+# ddr_RARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Right[:, j+2] - 2*R_Right[:, j+1] + R_Right[:, j])
+# ddr_LARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Left[:, j+2] - 2*R_Left[:, j+1] + R_Left[:, j])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_right' + str(i), w_ddr * optas.sumsqr(ddr_RARM_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_left' + str(i), w_ddr * optas.sumsqr(ddr_LARM_var_MPC[:, i]))
+
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.FeasTol': 1e-4, 'knitro.OptTol': 1e-4,
+ 'knitro.ftol':1e-4,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+ self.time_linspace_planner = np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner)
+ self.timebyT_planner = np.asarray(self.time_linspace_planner)/self.duration_MPC_planner
+ ### ---------------------------------------------------------
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 3000;
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+ #####################################################################################3
+ # get end-effector pose as parameters
+# pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+# ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+# pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+# ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+
+ pos_R_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ pos_L_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ R_Right_reasonal = builder_wholebodyMPC.add_parameter('R_Right_reasonal', 3, self.T_MPC_planner)
+ R_Left_reasonal = builder_wholebodyMPC.add_parameter('R_Left_reasonal', 3, self.T_MPC_planner)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+
+ fac = self.duration_MPC/self.duration_MPC_planner
+
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t[i]*fac)**j * (1-t[i]*fac)**(self.n_planner-j) * R_Right_reasonal[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t[i]*fac)**j * (1-t[i]*fac)**(self.n_planner-j) * R_Left_reasonal[:, j]
+
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+ F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ self.pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ self.ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.rotation_fnc_Right = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rotation_fnc_Left = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - Delta_p_Left_var_MPC[:, i]))
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Right_var_MPC[1, i] - F_ext_Right_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Left_var_MPC[1, i] - F_ext_Left_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[1, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[1, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+
+ #########################################################################################
+ # setup solver
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5,
+ 'knitro.ftol':1e-5, 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+ 'knitro.maxtime_real': 1.6e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4,
+ 'knitro.bar_penaltycons': 1, 'knitro.bar_penaltyrule':2,
+ 'knitro.bar_switchrule':2, 'knitro.linesearch': 1})
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare ft_sensor subscriber
+# self._ft_right_sub = rospy.Subscriber("/ft_right/raw/data", WrenchStamped, self.read_ft_sensor_right_data_cb)
+# self._ft_left_sub = rospy.Subscriber("/ft_left/raw/data", WrenchStamped, self.read_ft_sensor_left_data_cb)
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+# self._joint_pub = rospy.Publisher(self._pub_cmd_topic_name, Float64MultiArray, queue_size=10)
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+ self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ # declare two arm ee grasp actual force publisher
+# self._acc_sub = rospy.Subscriber("/chonk/acc_box", Float64MultiArray, self.read_box_acc_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, pos_Right[0], pos_Right[1], pos_Right[2], ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2], ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ # main execution
+ if(self._idx < self._steps):
+ time = rospy.get_time()
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self.ti_MPC_planner = 0 # time index of the MPC
+ self._idx += 1
+ pos_R_goal = []
+ pos_L_goal = []
+
+ for i in range(self.T_MPC_planner):
+ if(self.ti_MPC_planner <= self.duration):
+ self.ti_MPC_planner = self._t[self._idx-1] + self.dt_MPC_planner*i
+ if(self.ti_MPC_planner > self.duration):
+ self.ti_MPC_planner = self.duration
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC_planner).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC_planner).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_actual_Right = np.array(self.pos_fnc_Right(self.q_curr))[0:2]
+ r_actual_Left = np.array(self.pos_fnc_Left(self.q_curr))[0:2]
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC planner
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_middle': 0.5*(pos_R_goal+pos_L_goal), f'R_Right': pos_R_goal,
+ f'R_Left': pos_L_goal, f'r_ep': np.zeros(self.T_MPC_planner) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_middle': self.solution_MPC_planner[f'R_middle'],
+ f'R_Right': self.solution_MPC_planner[f'R_Right'],
+ f'R_Left': self.solution_MPC_planner[f'R_Left'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'] })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': pos_R_goal, 'pos_L': pos_L_goal,
+ 't': self.timebyT_planner,
+ 'init_r_position_Right': r_actual_Right,
+ 'init_r_position_Left': r_actual_Left} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_Right = np.asarray(self.solution_MPC_planner[f'R_Right'])
+ R_Left = np.asarray(self.solution_MPC_planner[f'R_Left'])
+
+ ### -----------------------------------------------------------
+ self.ti_MPC = 0
+ force_R_goal = []
+ force_L_goal = []
+ ori_R_goal = []
+ ori_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+
+# if(self._idx < self._steps):
+# if(self._correct_mux_selection):
+# # increment idx (in here counts starts with 1)
+# self.ti_MPC = 0 # time index of the MPC
+# self._idx += 1
+# pos_R_goal = []; ori_R_goal = []; pos_L_goal = []; ori_L_goal = []; force_R_goal = []; force_L_goal = [];
+## ddpos_box_goal = []
+# for i in range(self.T_MPC):
+# if(self.ti_MPC <= self.duration):
+# self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+# if(self.ti_MPC > self.duration):
+# self.ti_MPC = self.duration
+# try:
+# g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# pos_R_goal.append(g_rarm_ee_pos.tolist())
+# g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+## g_rarm_ee_ori[0] = np.sqrt(1-g_rarm_ee_ori[1]**2-g_rarm_ee_ori[2]**2-g_rarm_ee_ori[3]**2)
+# ori_R_goal.append(g_rarm_ee_ori.tolist())
+# g_larm_ee_pos = self._LARM_ee_Pos_trajectory(self.ti_MPC).flatten()
+# pos_L_goal.append(g_larm_ee_pos.tolist())
+# g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+## g_larm_ee_ori[0] = np.sqrt(1-g_larm_ee_ori[1]**2-g_larm_ee_ori[2]**2-g_larm_ee_ori[3]**2)
+# ori_L_goal.append(g_larm_ee_ori.tolist())
+# g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+# force_R_goal.append(g_rarm_ee_force.tolist())
+# g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+# force_L_goal.append(g_larm_ee_force.tolist())
+## g_box_ddpos = self._RARM_ee_ddPos_trajectory(self.ti_MPC).flatten()
+## ddpos_box_goal.append(g_box_ddpos.tolist())
+# except ValueError:
+# pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+# ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+# pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+# ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+# force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+# force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+## ddpos_box_goal.append(g_box_ddpos.tolist())
+# pos_R_goal = optas.np.array(pos_R_goal).T
+# ori_R_goal = optas.np.array(ori_R_goal).T
+# pos_L_goal = optas.np.array(pos_L_goal).T
+# ori_L_goal = optas.np.array(ori_L_goal).T
+# force_R_goal = optas.np.array(force_R_goal).T
+# force_L_goal = optas.np.array(force_L_goal).T
+## ddpos_box_goal = optas.np.array(ddpos_box_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+# self.Derivation_RARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1] - pos_R_goal[1, 0]
+# self.Derivation_LARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1] - pos_L_goal[1, 0]
+
+ self.G_Rotation_ee_right = self.rotation_fnc_Right(self.q_curr)
+ self.G_Rotation_ee_left = self.rotation_fnc_Left(self.q_curr)
+
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+
+# print(self.G_I_ee_r_conventional[3:6, 3:6])
+# print(self.G_I_ee_l_conventional[3:6, 3:6])
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ self.solver_wholebodyMPC.reset_parameters({'R_Right_reasonal': R_Right, 'R_Left_reasonal': R_Left,
+ 'ori_R_reasonal': ori_R_goal, 'ori_L_reasonal': ori_L_goal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'inertia_Right': self.G_I_ee_r_conventional[3:6, 3:6], 'inertia_Left': self.G_I_ee_l_conventional[3:6, 3:6],
+ 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_Left_actual': self.F_ext_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box} )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ q_next = np.zeros(self.ndof); p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += (1./self.duration_MPC) * self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+
+ ddq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-2):
+ ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+
+# self.acc_box = 0.5*(self.pos_Jac_fnc_Right(q_next) + self.pos_Jac_fnc_Left(q_next)) @ ddq_next
+# F_r = np.zeros(3)
+# F_l = np.zeros(3)
+# Delta_p_r = np.zeros(3)
+# dDelta_p_r = np.zeros(3)
+# ddDelta_p_r = np.zeros(3)
+# Delta_p_l = np.zeros(3)
+# dDelta_p_l = np.zeros(3)
+# ddDelta_p_l = np.zeros(3)
+# for j in range(self.T_MPC):
+# Delta_p_r += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+# Delta_p_l += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Left[:, j]
+# for j in range(self.T_MPC-1):
+# dDelta_p_r += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P_Right[:, j+1] - P_Right[:, j])
+# dDelta_p_l += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (P_Left[:, j+1] - P_Left[:, j])
+# for j in range(self.T_MPC-2):
+# ddDelta_p_r += self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+# ddDelta_p_l += self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+# #####################################################################################
+# F_r = self.F_ext_Right[3:6] + self.K_Right @ Delta_p_r
+# F_l = self.F_ext_Left[3:6] + self.K_Left @ Delta_p_l
+# F_r = self.F_ext_Right[3:6] + self.G_I_ee_r_conventional[3:6, 3:6] @ ddDelta_p_r + self.K_Right @ Delta_p_r + self.D_Right @ dDelta_p_r
+# F_l = self.F_ext_Left[3:6] + self.G_I_ee_l_conventional[3:6, 3:6] @ ddDelta_p_l + self.K_Left @ Delta_p_l + self.D_Left @ dDelta_p_l
+
+# print(F_r)
+# print(F_l)
+
+ # read current robot joint positions
+# self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+# self.Derivation_RARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q_next)).T[0][1] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1]
+# self.Derivation_LARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q_next)).T[0][1] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1]
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[1] = np.asarray(self.pos_fnc_Right(q_next)).T[0][1] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[1] = np.asarray(self.pos_fnc_Left(q_next)).T[0][1] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0][1]
+
+
+# self.Derivation_RARM_pos_start[1] = pos_R_goal[1, 0] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1]
+# self.Derivation_LARM_pos_start[1] = pos_L_goal[1, 0] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1]
+
+# print(self.Derivation_RARM_pos_start)
+# print(self.Derivation_LARM_pos_start)
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ print(rospy.get_time() - time)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+# def read_ft_sensor_right_data_cb(self, msg):
+# self.ft_right = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+# def read_ft_sensor_left_data_cb(self, msg):
+# self.ft_left = -np.asarray([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z, msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+ def read_right_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5] ])
+ self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+ def read_left_ee_grasp_ft_data_cb(self, msg):
+# self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], msg.data[3], msg.data[4], msg.data[5] ])
+ self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+# def read_box_acc_data_cb(self, msg):
+# self.acc_box = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.py
new file mode 100755
index 0000000..df0cf48
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory.py
@@ -0,0 +1,918 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+# self.dt_MPC_planner = 0.1 # time step
+ self.T_MPC_planner = 13 # T is number of time steps
+# self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_Right = builder_wholebodyMPC_planner.add_decision_variables('R_Right', 3, self.T_MPC_planner)
+ R_Left = builder_wholebodyMPC_planner.add_decision_variables('R_Left', 3, self.T_MPC_planner)
+# R_middle = builder_wholebodyMPC_planner.add_decision_variables('R_middle', 3, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+ r_ep_start = builder_wholebodyMPC_planner.add_decision_variables('r_ep_start', 12)
+
+ duration_MPC_planner = builder_wholebodyMPC_planner.add_parameter('duration_MPC_planner', 1)
+
+ t = np.linspace(0., 1., self.T_MPC_planner)
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+ # Add parameters
+# init_r_position_middle = builder_wholebodyMPC_planner.add_parameter('init_r_position_middle', 2)
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 3)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 3)
+
+ init_dr_position_Right = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Right', 3)
+ init_dr_position_Left = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Left', 3)
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+# r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Right[:, j]
+ r_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Left[:, j]
+ r_middle_var_MPC[:, i] = 0.5*(r_RARM_var_MPC[:, i]+r_LARM_var_MPC[:, i])
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_z' + str(i), optas.sumsqr(r_RARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_z' + str(i), optas.sumsqr(r_LARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_equality_constraint('middle' +str(i), r_middle_var_MPC[:, i], rhs=0.5*(r_RARM_var_MPC[:, i]+r_LARM_var_MPC[:, i]))
+
+
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - r_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_RARM_var_MPC[0, i], rhs=r_LARM_var_MPC[0, i])
+
+ # optimization cost: close to target
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_x', optas.sumsqr(r_middle_var_MPC[0, -1] - 0.5*(pos_R[0] + pos_L[0])))
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_y', 10*optas.sumsqr(r_middle_var_MPC[1, -1] - 0.5*(pos_R[1] + pos_L[1])))
+# builder_wholebodyMPC_planner.add_equality_constraint('Final_middle', r_middle_var_MPC[:, self.T_MPC_planner-1], rhs=0.5*(pos_R+pos_L))
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_arm', r_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_arm', r_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_L)
+
+
+
+
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', 10*optas.sumsqr(r_ep))
+
+ # add position constraint at the beginning state
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[:, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Right', R_Right[:, 0], rhs=init_r_position_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Left', R_Left[:, 0], rhs=init_r_position_Left)
+
+ for i in range(self.T_MPC_planner):
+ if(i<(self.T_MPC_planner -1)):
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance' + str(i), 10 * optas.sumsqr(R_Right[:, i+1] - R_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance' + str(i), 10 * optas.sumsqr(R_Left[:, i+1] - R_Left[:, i]))
+ if(i<(self.T_MPC_planner -2)):
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance' + str(i), 10 * optas.sumsqr(R_Right[:, i+2]-2*R_Right[:, i+1] + R_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance' + str(i), 10 * optas.sumsqr(R_Left[:, i+2]-2*R_Left[:, i+1] + R_Left[:, i]))
+
+# dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ w_dr = duration_MPC_planner**2 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+# dr_middle_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+ dr_RARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Right[:, j+1] - R_Right[:, j])
+ dr_LARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Left[:, j+1] - R_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_right' + str(i), w_dr * optas.sumsqr(dr_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_left' + str(i), w_dr * optas.sumsqr(dr_LARM_var_MPC[:, i]))
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_middle', dr_middle_var_MPC[:, 0], rhs=0.5*(init_dr_position_Right + init_dr_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Right', dr_RARM_var_MPC[0:2, 0], rhs=init_dr_position_Right[0:2])
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Left', dr_LARM_var_MPC[0:2, 0], rhs=init_dr_position_Left[0:2])
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dr_middle', dr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_right', dr_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_left', dr_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+
+ lower_velocity = np.array([-1., -1., -0.5]); upper_velocity = np.array([1., 1., 0.5]);
+ for i in range(self.T_MPC_planner-1):
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity-r_ep_start[0:3], mid=(1./duration_MPC_planner) * self.n_planner * (R_Right[:, i+1] - R_Right[:, i]), rhs=upper_velocity+r_ep_start[0:3])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity-r_ep_start[3:6], mid=(1./duration_MPC_planner) * self.n_planner * (R_Left[:, i+1] - R_Left[:, i]), rhs=upper_velocity+r_ep_start[3:6])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_Right[:, i+1] - R_Right[:, i]), rhs=upper_velocity)
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_Left[:, i+1] - R_Left[:, i]), rhs=upper_velocity)
+
+# ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ w_ddr = duration_MPC_planner**4 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+# ddr_middle_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+ ddr_RARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Right[:, j+2] - 2*R_Right[:, j+1] + R_Right[:, j])
+ ddr_LARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Left[:, j+2] - 2*R_Left[:, j+1] + R_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_right' + str(i), w_ddr * optas.sumsqr(ddr_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_left' + str(i), w_ddr * optas.sumsqr(ddr_LARM_var_MPC[:, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_middle', ddr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_right', ddr_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_left', ddr_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+
+ lower_acceleration = np.array([-1.5, -1.5, -0.5]); upper_acceleration = np.array([1.5, 1.5, 0.5]);
+ for i in range(self.T_MPC_planner-2):
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration-r_ep_start[6:9], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Right[:, i+2] - 2 * R_Right[:, i+1] + R_Right[:, i]), rhs=upper_acceleration+r_ep_start[6:9])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration-r_ep_start[9:12], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Left[:, i+2] - 2 * R_Left[:, i+1] + R_Left[:, i]), rhs=upper_acceleration+r_ep_start[9:12])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Right[:, i+2] - 2 * R_Right[:, i+1] + R_Right[:, i]), rhs=upper_acceleration)
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Left[:, i+2] - 2 * R_Left[:, i+1] + R_Left[:, i]), rhs=upper_acceleration)
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right1', R_Right[:,8], rhs=R_Right[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right2', R_Right[:,9], rhs=R_Right[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left1', R_Left[:,8], rhs=R_Left[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left2', R_Left[:,9], rhs=R_Left[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_right', R_Right[:,self.T_MPC_planner-4], rhs=R_Right[:,self.T_MPC_planner-1])
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_left', R_Left[:,self.T_MPC_planner-4], rhs=R_Left[:,self.T_MPC_planner-1])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep_start', 100*optas.sumsqr(r_ep_start))
+
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0,
+ 'print_time': 0,
+# 'knitro.act_qpalg': 1,
+ 'knitro.FeasTol': 5e-4, 'knitro.OptTol': 5e-4, 'knitro.ftol':5e-4,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+
+ ### ---------------------------------------------------------
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 3000;
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+ #####################################################################################3
+ # get end-effector pose as parameters
+# pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+# ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+# pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+# ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+
+# pos_R_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+# pos_L_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ pos_R_reasonal = builder_wholebodyMPC.add_parameter('pos_R_reasonal', 3, self.T_MPC)
+ pos_L_reasonal = builder_wholebodyMPC.add_parameter('pos_L_reasonal', 3, self.T_MPC)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+ F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ self.pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ self.ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.rotation_fnc_Right = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rotation_fnc_Left = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - 2.5*Delta_p_Right_var_MPC[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - 2.5*Delta_p_Left_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - 2.5*Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - 2.5*Delta_p_Left_var_MPC[:, i]))
+
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_x' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[0]-pos_R_reasonal[0, i] ))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_y' + str(i), 5*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[1]-pos_R_reasonal[1, i] - Delta_p_Right_var_MPC[1, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_z' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[2]-pos_R_reasonal[2, i] ))
+
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_x' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[0]-pos_L_reasonal[0, i] ))
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_y' + str(i), 5*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[1]-pos_L_reasonal[1, i] - Delta_p_Left_var_MPC[1, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_z' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[2]-pos_L_reasonal[2, i] ))
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Right_var_MPC[1, i] - F_ext_Right_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Left_var_MPC[1, i] - F_ext_Left_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+# builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[1, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[1, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = self.duration_MPC**2 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = self.duration_MPC**4 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+
+ #########################################################################################
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5,
+ 'knitro.ftol':1e-5, 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4,
+ 'knitro.bar_penaltycons': 1, 'knitro.bar_penaltyrule':2,
+ 'knitro.bar_switchrule':2, 'knitro.linesearch': 1})
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+ self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ self.pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ self.pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ self.ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ self.ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, self.pos_Right[0], self.pos_Right[1], self.pos_Right[2], self.ori_Right[0], self.ori_Right[1], self.ori_Right[2], self.ori_Right[3],
+ self.pos_Left[0], self.pos_Left[1], self.pos_Left[2], self.ori_Left[0], self.ori_Left[1], self.ori_Left[2], self.ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self.duration_MPC_planner = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = self.pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = self.ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = self.pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = self.ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ # main execution
+ if(self._idx < self._steps):
+ time_begin = rospy.get_time()
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_actual_Right = np.array(self.pos_fnc_Right(self.q_curr))[0:3]
+ r_actual_Left = np.array(self.pos_fnc_Left(self.q_curr))[0:3]
+
+ dr_actual_Right = np.asarray(self.pos_Jac_fnc_Right(self.q_curr)) @ self.dq_curr
+ dr_actual_Left = np.asarray(self.pos_Jac_fnc_Left(self.q_curr)) @ self.dq_curr
+
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC planner
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ pos_R_goal = []; pos_L_goal = [];
+ _t = np.asarray(np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner))
+ for i in range(self.T_MPC_planner):
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': pos_R_goal,
+ f'R_Left': pos_L_goal, f'r_ep': np.zeros(self.T_MPC_planner),
+ f'r_ep_start': np.zeros(12)
+ })
+# self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': np.zeros((3, self.T_MPC_planner)),
+# f'R_Left': np.zeros((3, self.T_MPC_planner)), f'r_ep': np.zeros(self.T_MPC_planner) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': self.solution_MPC_planner[f'R_Right'],
+ f'R_Left': self.solution_MPC_planner[f'R_Left'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'],
+ f'r_ep_start': self.solution_MPC_planner[f'r_ep_start']
+ })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': self.pos_Right, 'pos_L': self.pos_Left,
+ 'init_r_position_Right': r_actual_Right,
+ 'init_r_position_Left': r_actual_Left,
+ 'init_dr_position_Right': dr_actual_Right,
+ 'init_dr_position_Left': dr_actual_Left,
+ 'duration_MPC_planner': self.duration_MPC_planner} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_Right = np.asarray(self.solution_MPC_planner[f'R_Right'])
+ R_Left = np.asarray(self.solution_MPC_planner[f'R_Left'])
+
+ pos_R_reasonal = np.zeros((3, self.T_MPC))
+ pos_L_reasonal = np.zeros((3, self.T_MPC))
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if((self.ti_MPC - self._t[self._idx-1]) <= self.duration_MPC_planner and self.duration_MPC_planner>= 2./self._freq):
+ t_nomalized = (self.ti_MPC - self._t[self._idx-1])/self.duration_MPC_planner
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Left[:, j]
+ else:
+ t_nomalized = 1.
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Left[:, j]
+# print(self._t[self._idx-1])
+# print(self.duration_MPC_planner)
+# print(R_Right[2, :])
+# print(pos_R_reasonal[2, :])
+# print('left')
+# print(R_Left[2, :])
+# print(pos_L_reasonal[2, :])
+ ### -----------------------------------------------------------
+ self.ti_MPC = 0
+ force_R_goal = []
+ force_L_goal = []
+ ori_R_goal = []
+ ori_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ self.G_Rotation_ee_right = self.rotation_fnc_Right(self.q_curr)
+ self.G_Rotation_ee_left = self.rotation_fnc_Left(self.q_curr)
+
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R_reasonal': pos_R_reasonal, 'pos_L_reasonal': pos_L_reasonal,
+ 'ori_R_reasonal': ori_R_goal, 'ori_L_reasonal': ori_L_goal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'inertia_Right': self.G_I_ee_r_conventional[3:6, 3:6], 'inertia_Left': self.G_I_ee_l_conventional[3:6, 3:6],
+ 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_Left_actual': self.F_ext_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box } )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ self.q_next = np.zeros(self.ndof); p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ self.q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += (1./self.duration_MPC) * self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ self.dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ self.dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+
+ self.ddq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-2):
+ self.ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[1] = np.asarray(self.pos_fnc_Right(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[1] = np.asarray(self.pos_fnc_Left(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0][1]
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., self.dq_next[2]])
+ Global_v_b = np.asarray([self.dq_next[0], self.dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+ self.duration_MPC_planner = self.duration - self._idx/self._freq
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = self.q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = self.q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = self.ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ print(rospy.get_time() - time_begin)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+ def read_right_ee_grasp_ft_data_cb(self, msg):
+ self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+ def read_left_ee_grasp_ft_data_cb(self, msg):
+ self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_notgood.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_notgood.py
new file mode 100755
index 0000000..ecd7126
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_notgood.py
@@ -0,0 +1,960 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC_planner'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+# self.dt_MPC_planner = 0.1 # time step
+ self.T_MPC_planner = 15 # T is number of time steps
+# self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_Right = builder_wholebodyMPC_planner.add_decision_variables('R_Right', 3, self.T_MPC_planner)
+ R_Left = builder_wholebodyMPC_planner.add_decision_variables('R_Left', 3, self.T_MPC_planner)
+# R_middle = builder_wholebodyMPC_planner.add_decision_variables('R_middle', 3, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+# r_ep_v = builder_wholebodyMPC_planner.add_decision_variables('r_ep_v', 3, self.T_MPC_planner-1)
+# r_ep_dv = builder_wholebodyMPC_planner.add_decision_variables('r_ep_dv', 3, self.T_MPC_planner-2)
+
+
+
+ duration_MPC_planner = builder_wholebodyMPC_planner.add_parameter('duration_MPC_planner', 1)
+
+ t = np.linspace(0., 1., self.T_MPC_planner)
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+ # Add parameters
+# init_r_position_middle = builder_wholebodyMPC_planner.add_parameter('init_r_position_middle', 2)
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 3)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 3)
+
+ init_dr_position_Right = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Right', 3)
+ init_dr_position_Left = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Left', 3)
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+# r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Right[:, j]
+ r_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_Left[:, j]
+ r_middle_var_MPC[:, i] = 0.5*(r_RARM_var_MPC[:, i]+r_LARM_var_MPC[:, i])
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_z' + str(i), optas.sumsqr(r_RARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_z' + str(i), optas.sumsqr(r_LARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_equality_constraint('middle' +str(i), r_middle_var_MPC[:, i], rhs=0.5*(r_RARM_var_MPC[:, i]+r_LARM_var_MPC[:, i]))
+
+
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_RARM_var_MPC[0, i] - r_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_RARM_var_MPC[0, i], rhs=r_LARM_var_MPC[0, i])
+
+ # optimization cost: close to target
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_x', optas.sumsqr(r_middle_var_MPC[0, -1] - 0.5*(pos_R[0] + pos_L[0])))
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_y', 10*optas.sumsqr(r_middle_var_MPC[1, -1] - 0.5*(pos_R[1] + pos_L[1])))
+# builder_wholebodyMPC_planner.add_equality_constraint('Final_middle', r_middle_var_MPC[:, self.T_MPC_planner-1], rhs=0.5*(pos_R+pos_L))
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_arm', r_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_arm', r_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_L)
+
+
+
+
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', 100*optas.sumsqr(r_ep))
+
+
+ # add position constraint at the beginning state
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[:, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Right', R_Right[:, 0], rhs=init_r_position_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Left', R_Left[:, 0], rhs=init_r_position_Left)
+
+ for i in range(self.T_MPC_planner):
+ if(i<(self.T_MPC_planner -1)):
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance' + str(i), 100*optas.sumsqr(R_Right[:, i+1] - R_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance' + str(i), 100*optas.sumsqr(R_Left[:, i+1] - R_Left[:, i]))
+ if(i<(self.T_MPC_planner -2)):
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance' + str(i), 100*optas.sumsqr(R_Right[:, i+2]-2*R_Right[:, i+1] + R_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance' + str(i), 100*optas.sumsqr(R_Left[:, i+2]-2*R_Left[:, i+1] + R_Left[:, i]))
+
+
+# for i in range(self.T_MPC_planner):
+# if(i<(self.T_MPC_planner -1)):
+# builder_wholebodyMPC_planner.add_cost_term('Right_distance_x' + str(i), 0.05 * optas.sumsqr(R_Right[0, i+1] - R_Right[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_distance_y' + str(i), 0.05 * optas.sumsqr(R_Right[1, i+1] - R_Right[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_distance_z' + str(i), 0.05 * optas.sumsqr(R_Right[2, i+1] - R_Right[2, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('Left_distance_x' + str(i), 0.05 * optas.sumsqr(R_Left[0, i+1] - R_Left[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_distance_y' + str(i), 0.05 * optas.sumsqr(R_Left[1, i+1] - R_Left[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_distance_z' + str(i), 0.05 * optas.sumsqr(R_Left[2, i+1] - R_Left[2, i]))
+
+# if(i<(self.T_MPC_planner -2)):
+# builder_wholebodyMPC_planner.add_cost_term('dRight_distance_x' + str(i), 0.05 * optas.sumsqr(R_Right[0, i+2]-2*R_Right[0, i+1] + R_Right[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('dRight_distance_y' + str(i), 0.05 * optas.sumsqr(R_Right[1, i+2]-2*R_Right[1, i+1] + R_Right[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('dRight_distance_z' + str(i), 0.05 * optas.sumsqr(R_Right[2, i+2]-2*R_Right[2, i+1] + R_Right[2, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_x' + str(i), 0.05 * optas.sumsqr(R_Left[0, i+2]-2*R_Left[0, i+1] + R_Left[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_y' + str(i), 0.05 * optas.sumsqr(R_Left[1, i+2]-2*R_Left[1, i+1] + R_Left[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('dLeft_distance_z' + str(i), 0.05 * optas.sumsqr(R_Left[2, i+2]-2*R_Left[2, i+1] + R_Left[2, i]))
+
+
+# dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ w_dr = 0.05/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+# dr_middle_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+ dr_RARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Right[:, j+1] - R_Right[:, j])
+ dr_LARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_Left[:, j+1] - R_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_right' + str(i), w_dr * optas.sumsqr(dr_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_left' + str(i), w_dr * optas.sumsqr(dr_LARM_var_MPC[:, i]))
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_middle', dr_middle_var_MPC[:, 0], rhs=0.5*(init_dr_position_Right + init_dr_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Right', dr_RARM_var_MPC[:, 0], rhs=init_dr_position_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Left', dr_LARM_var_MPC[:, 0], rhs=init_dr_position_Left)
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dr_middle', dr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_right', dr_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_left', dr_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+
+# lower_velocity = np.array([-1.5, -1.5, -0.3]); upper_velocity = np.array([1.5, 1.5, 0.3]);
+# for i in range(self.T_MPC_planner-1):
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_Right[:, i+1] - R_Right[:, i]), rhs=upper_velocity)
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_Left[:, i+1] - R_Left[:, i]), rhs=upper_velocity)
+
+# ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+
+ w_ddr = 0.05/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+# ddr_middle_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+ ddr_RARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Right[:, j+2] - 2*R_Right[:, j+1] + R_Right[:, j])
+ ddr_LARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_Left[:, j+2] - 2*R_Left[:, j+1] + R_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_right' + str(i), w_ddr * optas.sumsqr(ddr_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_left' + str(i), w_ddr * optas.sumsqr(ddr_LARM_var_MPC[:, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_middle', ddr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_right', ddr_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_left', ddr_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+
+# lower_acceleration = np.array([-1.5, -1.5, -0.5]); upper_acceleration = np.array([1.5, 1.5, 0.5]);
+# for i in range(self.T_MPC_planner-2):
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Right[:, i+2] - 2 * R_Right[:, i+1] + R_Right[:, i]), rhs=upper_acceleration)
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_Left[:, i+2] - 2 * R_Left[:, i+1] + R_Left[:, i]), rhs=upper_acceleration)
+
+
+# builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep_v', 10000*optas.sumsqr(r_ep_v))
+# builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep_dv', 10000*optas.sumsqr(r_ep_dv))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right1', R_Right[:,8], rhs=R_Right[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right2', R_Right[:,9], rhs=R_Right[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left1', R_Left[:,8], rhs=R_Left[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left2', R_Left[:,9], rhs=R_Left[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_right', R_Right[:,self.T_MPC_planner-4], rhs=R_Right[:,self.T_MPC_planner-1])
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_left', R_Left[:,self.T_MPC_planner-4], rhs=R_Left[:,self.T_MPC_planner-1])
+ # setup solver
+# self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+## 'knitro.OutLev': 0,
+# 'print_time': 0,
+# 'knitro.act_qpalg': 1,
+# 'knitro.FeasTol': 1e-4, 'knitro.OptTol': 1e-4,
+# 'knitro.ftol':1e-4,
+# 'knitro.algorithm':3, 'knitro.linsolver':2,
+## 'knitro.maxtime_real': 1.0e-2,
+# 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+# 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+# } )
+ self.solution_MPC_planner = None
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.FeasTol': 1e-4, 'knitro.OptTol': 1e-4,
+ 'knitro.ftol':1e-4,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+
+ ### ---------------------------------------------------------
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 3000;
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+ #####################################################################################3
+ # get end-effector pose as parameters
+ pos_R_reasonal = builder_wholebodyMPC.add_parameter('pos_R_reasonal', 3, self.T_MPC)
+ pos_L_reasonal = builder_wholebodyMPC.add_parameter('pos_L_reasonal', 3, self.T_MPC)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+ F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ self.pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ self.ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.rotation_fnc_Right = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rotation_fnc_Left = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += self.BC(self.n-2, j)**2 * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), 0.03*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - Delta_p_Right_var_MPC[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), 0.03*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - Delta_p_Left_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i]) - pos_R_reasonal[:, i] - Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i]) - pos_L_reasonal[:, i] - Delta_p_Left_var_MPC[:, i]))
+
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_x' + str(i), 0.5*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[0]-pos_R_reasonal[0, i] ))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_y' + str(i), 0.5*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[1]-pos_R_reasonal[1, i] - Delta_p_Right_var_MPC[1, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_z' + str(i), 0.5*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[2]-pos_R_reasonal[2, i] ))
+
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_x' + str(i), 0.5*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[0]-pos_L_reasonal[0, i] ))
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_y' + str(i), 0.5*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[1]-pos_L_reasonal[1, i] - Delta_p_Left_var_MPC[1, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_z' + str(i), 0.5*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[2]-pos_L_reasonal[2, i] ))
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Right_var_MPC[1, i] - F_ext_Right_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Left_var_MPC[1, i] - F_ext_Left_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[1, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[1, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.0005/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+
+ #########################################################################################
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5,
+ 'knitro.ftol':1e-5, 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.6e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4,
+ 'knitro.bar_penaltycons': 1, 'knitro.bar_penaltyrule':2,
+ 'knitro.bar_switchrule':2, 'knitro.linesearch': 1})
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+ self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ self.pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ self.pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ self.ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ self.ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, self.pos_Right[0], self.pos_Right[1], self.pos_Right[2], self.ori_Right[0], self.ori_Right[1], self.ori_Right[2], self.ori_Right[3],
+ self.pos_Left[0], self.pos_Left[1], self.pos_Left[2], self.ori_Left[0], self.ori_Left[1], self.ori_Left[2], self.ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self.duration_MPC_planner = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = self.pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = self.ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = self.pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = self.ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ # main execution
+ if(self._idx < self._steps):
+ time = rospy.get_time()
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_actual_Right = np.array(self.pos_fnc_Right(self.q_curr))[0:3]
+ r_actual_Left = np.array(self.pos_fnc_Left(self.q_curr))[0:3]
+
+ dr_actual_Right = np.asarray(self.pos_Jac_fnc_Right(self.q_curr)) @ self.dq_curr
+ dr_actual_Left = np.asarray(self.pos_Jac_fnc_Left(self.q_curr)) @ self.dq_curr
+
+
+
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC planner
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ pos_R_goal = []; pos_L_goal = [];
+ _t = np.asarray(np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner))
+ for i in range(self.T_MPC_planner):
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': pos_R_goal, f'R_Left': pos_L_goal,
+ f'r_ep': np.zeros(self.T_MPC_planner),
+# f'r_ep_v': np.zeros((3, self.T_MPC_planner-1)), f'r_ep_dv': np.zeros((3, self.T_MPC_planner-2))
+ })
+# self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': np.zeros((3, self.T_MPC_planner)),
+# f'R_Left': np.zeros((3, self.T_MPC_planner)), f'r_ep': np.zeros(self.T_MPC_planner) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_Right': self.solution_MPC_planner[f'R_Right'],
+ f'R_Left': self.solution_MPC_planner[f'R_Left'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'],
+# f'r_ep_v': self.solution_MPC_planner[f'r_ep_v'],
+# f'r_ep_dv': self.solution_MPC_planner[f'r_ep_dv']
+ })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': self.pos_Right, 'pos_L': self.pos_Left,
+ 'init_r_position_Right': r_actual_Right,
+ 'init_r_position_Left': r_actual_Left,
+ 'init_dr_position_Right': dr_actual_Right,
+ 'init_dr_position_Left': dr_actual_Left,
+ 'duration_MPC_planner': self.duration_MPC_planner} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_Right = np.asarray(self.solution_MPC_planner[f'R_Right'])
+ R_Left = np.asarray(self.solution_MPC_planner[f'R_Left'])
+
+ pos_R_reasonal = np.zeros((3, self.T_MPC))
+ pos_L_reasonal = np.zeros((3, self.T_MPC))
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if((self.ti_MPC - self._t[self._idx-1]) <= self.duration_MPC_planner and self.duration_MPC_planner>(1./self._freq)):
+ t_nomalized = (self.ti_MPC - self._t[self._idx-1])/self.duration_MPC_planner
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Left[:, j]
+ else:
+ t_nomalized = 1.
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_Left[:, j]
+# print(self._t[self._idx-1])
+# print(self.duration_MPC_planner)
+# print(R_Right)
+# dR_Right = np.zeros((3, self.T_MPC_planner-1))
+
+# for j in range(self.T_MPC_planner-1):
+# dR_Right[:, j] = (1./self.duration_MPC_planner) * self.n_planner * (R_Right[:, j+1] - R_Right[:, j])
+# print(dR_Right)
+# print(pos_R_reasonal)
+# print('left')
+# print(R_Left)
+# print(pos_L_reasonal[2, :])
+ ### -----------------------------------------------------------
+ self.ti_MPC = 0
+ force_R_goal = []
+ force_L_goal = []
+ ori_R_goal = []
+ ori_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+
+# # read current robot joint positions
+# self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+# self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ self.G_Rotation_ee_right = self.rotation_fnc_Right(self.q_curr)
+ self.G_Rotation_ee_left = self.rotation_fnc_Left(self.q_curr)
+
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R_reasonal': pos_R_reasonal, 'pos_L_reasonal': pos_L_reasonal,
+ 'ori_R_reasonal': ori_R_goal, 'ori_L_reasonal': ori_L_goal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'inertia_Right': self.G_I_ee_r_conventional[3:6, 3:6], 'inertia_Left': self.G_I_ee_l_conventional[3:6, 3:6],
+ 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_Left_actual': self.F_ext_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box } )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ self.q_next = np.zeros(self.ndof); p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ self.q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += (1./self.duration_MPC) * self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ self.dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ self.dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+
+ self.ddq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-2):
+ self.ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+
+ # read current robot joint positions
+# self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[1] = np.asarray(self.pos_fnc_Right(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[1] = np.asarray(self.pos_fnc_Left(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0][1]
+
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., self.dq_next[2]])
+ Global_v_b = np.asarray([self.dq_next[0], self.dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+# print(p_right)
+# print(self.Derivation_RARM_pos_start)
+
+ self.duration_MPC_planner = self.duration - self._idx/self._freq
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = self.q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = self.q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = self.ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ print(rospy.get_time() - time)
+
+
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+ def read_right_ee_grasp_ft_data_cb(self, msg):
+ self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+ def read_left_ee_grasp_ft_data_cb(self, msg):
+ self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py
new file mode 100755
index 0000000..6b37150
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation.py
@@ -0,0 +1,1011 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+# self.dt_MPC_planner = 0.1 # time step
+ self.T_MPC_planner = 12 # T is number of time steps
+# self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_pos_Right = builder_wholebodyMPC_planner.add_decision_variables('R_pos_Right', 3, self.T_MPC_planner)
+ R_pos_Left = builder_wholebodyMPC_planner.add_decision_variables('R_pos_Left', 3, self.T_MPC_planner)
+ R_ori_Right = builder_wholebodyMPC_planner.add_decision_variables('R_ori_Right', 4, self.T_MPC_planner)
+ R_ori_Left = builder_wholebodyMPC_planner.add_decision_variables('R_ori_Left', 4, self.T_MPC_planner)
+# R_middle = builder_wholebodyMPC_planner.add_decision_variables('R_middle', 3, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+ r_ep_start = builder_wholebodyMPC_planner.add_decision_variables('r_ep_start', 12)
+ r_ori_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ori_ep', 2, self.T_MPC_planner)
+
+
+ duration_MPC_planner = builder_wholebodyMPC_planner.add_parameter('duration_MPC_planner', 1)
+
+ self.pos_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_position_function(link=self._link_ee_left)
+ self.ori_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_quaternion_function(link=self._link_ee_left)
+
+ self.pos_Jac_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ self.ori_Jac_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_right)
+ self.ori_Jac_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_left)
+
+ t = np.linspace(0., 1., self.T_MPC_planner)
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+ # Add parameters
+# init_r_position_middle = builder_wholebodyMPC_planner.add_parameter('init_r_position_middle', 2)
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 3)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 3)
+ init_r_orientation_Right = builder_wholebodyMPC_planner.add_parameter('init_r_orientation_Right', 4)
+ init_r_orientation_Left = builder_wholebodyMPC_planner.add_parameter('init_r_orientation_Left', 4)
+
+ init_dr_position_Right = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Right', 3)
+ init_dr_position_Left = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Left', 3)
+ init_dr_orientation_Right = builder_wholebodyMPC_planner.add_parameter('init_dr_orientation_Right', 4)
+ init_dr_orientation_Left = builder_wholebodyMPC_planner.add_parameter('init_dr_orientation_Left', 4)
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3)
+ ori_R = builder_wholebodyMPC_planner.add_parameter('ori_R', 4)
+ ori_L = builder_wholebodyMPC_planner.add_parameter('ori_L', 4)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ r_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+# r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_pos_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_pos_Right[:, j]
+ r_pos_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_pos_Left[:, j]
+ r_ori_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_ori_Right[:, j]
+ r_ori_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_ori_Left[:, j]
+ r_middle_var_MPC[:, i] = 0.5*(r_pos_RARM_var_MPC[:, i]+r_pos_LARM_var_MPC[:, i])
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_z' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_z' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_equality_constraint('middle' +str(i), r_middle_var_MPC[:, i], rhs=0.5*(r_pos_RARM_var_MPC[:, i]+r_pos_LARM_var_MPC[:, i]))
+# builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_right' + str(i), lhs=optas.sumsqr(R_ori_Right[:, i]), rhs=1. + r_ori_ep[0, i])
+# builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_left' + str(i), lhs=optas.sumsqr(R_ori_Left[:, i]), rhs=1. + r_ori_ep[1, i])
+ builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_right' + str(i), lhs=optas.sumsqr(R_ori_Right[:, i]), rhs=1.)
+ builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_left' + str(i), lhs=optas.sumsqr(R_ori_Left[:, i]), rhs=1.)
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[0, i] - r_pos_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_pos_RARM_var_MPC[0, i], rhs=r_pos_LARM_var_MPC[0, i])
+
+ # optimization cost: close to target
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_x', optas.sumsqr(r_middle_var_MPC[0, -1] - 0.5*(pos_R[0] + pos_L[0])))
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_y', 10*optas.sumsqr(r_middle_var_MPC[1, -1] - 0.5*(pos_R[1] + pos_L[1])))
+# builder_wholebodyMPC_planner.add_equality_constraint('Final_middle', r_middle_var_MPC[:, self.T_MPC_planner-1], rhs=0.5*(pos_R+pos_L))
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_arm', r_pos_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_arm', r_pos_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_L)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_ori_arm', r_ori_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=ori_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_ori_arm', r_ori_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=ori_L)
+
+
+
+
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', 10*optas.sumsqr(r_ep))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ori_ep', 100*optas.sumsqr(r_ori_ep))
+
+ # add position constraint at the beginning state
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[:, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Right', R_pos_Right[:, 0], rhs=init_r_position_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_Left', R_pos_Left[:, 0], rhs=init_r_position_Left)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_ori_Right', R_ori_Right[:, 0], rhs=init_r_orientation_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_ori_Left', R_ori_Left[:, 0], rhs=init_r_orientation_Left)
+
+ for i in range(self.T_MPC_planner):
+ if(i<(self.T_MPC_planner -1)):
+ builder_wholebodyMPC_planner.add_cost_term('Right_distance' + str(i), 20 * optas.sumsqr(R_pos_Right[:, i+1] - R_pos_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_distance' + str(i), 20 * optas.sumsqr(R_pos_Left[:, i+1] - R_pos_Left[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Right_ori_distance' + str(i), 10 * optas.sumsqr(R_ori_Right[:, i+1] - R_ori_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_ori_distance' + str(i), 10 * optas.sumsqr(R_ori_Left[:, i+1] - R_ori_Left[:, i]))
+ if(i<(self.T_MPC_planner -2)):
+ builder_wholebodyMPC_planner.add_cost_term('dRight_distance' + str(i), 20 * optas.sumsqr(R_pos_Right[:, i+2]-2*R_pos_Right[:, i+1] + R_pos_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_distance' + str(i), 20 * optas.sumsqr(R_pos_Left[:, i+2] -2*R_pos_Left[:, i+1] + R_pos_Left[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dRight_ori_distance' + str(i), 10 * optas.sumsqr(R_ori_Right[:, i+2]-2*R_ori_Right[:, i+1] + R_ori_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_ori_distance' + str(i), 10 * optas.sumsqr(R_ori_Left[:, i+2] -2*R_ori_Left[:, i+1] + R_ori_Left[:, i]))
+
+# dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ dr_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+ w_dr = duration_MPC_planner**2 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+# dr_middle_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+ dr_pos_RARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_pos_Right[:, j+1] - R_pos_Right[:, j])
+ dr_pos_LARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_pos_Left[:, j+1] - R_pos_Left[:, j])
+ dr_ori_RARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_ori_Right[:, j+1] - R_ori_Right[:, j])
+ dr_ori_LARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_ori_Left[:, j+1] - R_ori_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_right' + str(i), w_dr * optas.sumsqr(dr_pos_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_left' + str(i), w_dr * optas.sumsqr(dr_pos_LARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_ori_right' + str(i), w_dr * optas.sumsqr(dr_ori_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_ori_left' + str(i), w_dr * optas.sumsqr(dr_ori_LARM_var_MPC[:, i]))
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_middle', dr_middle_var_MPC[:, 0], rhs=0.5*(init_dr_position_Right + init_dr_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Right', dr_pos_RARM_var_MPC[0:2, 0], rhs=init_dr_position_Right[0:2])
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_Left', dr_pos_LARM_var_MPC[0:2, 0], rhs=init_dr_position_Left[0:2])
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Right', dr_ori_RARM_var_MPC[:, 0], rhs=init_dr_orientation_Right)
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Left', dr_ori_LARM_var_MPC[:, 0], rhs=init_dr_orientation_Left)
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dr_middle', dr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_right', dr_pos_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_left', dr_pos_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_ori_right', dr_ori_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_ori_left', dr_ori_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+
+ lower_velocity = np.array([-1., -1., -0.5]); upper_velocity = np.array([1., 1., 0.5]);
+ for i in range(self.T_MPC_planner-1):
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity-r_ep_start[0:3], mid=(1./duration_MPC_planner) * self.n_planner * (R_pos_Right[:, i+1] - R_pos_Right[:, i]), rhs=upper_velocity+r_ep_start[0:3])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity-r_ep_start[3:6], mid=(1./duration_MPC_planner) * self.n_planner * (R_pos_Left[:, i+1] - R_pos_Left[:, i]), rhs=upper_velocity+r_ep_start[3:6])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_pos_Right[:, i+1] - R_pos_Right[:, i]), rhs=upper_velocity)
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity, mid=(1./duration_MPC_planner) * self.n_planner * (R_pos_Left[:, i+1] - R_pos_Left[:, i]), rhs=upper_velocity)
+
+# ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ ddr_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+ w_ddr = duration_MPC_planner**4 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+# ddr_middle_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+ ddr_pos_RARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_pos_Right[:, j+2] - 2*R_pos_Right[:, j+1] + R_pos_Right[:, j])
+ ddr_pos_LARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_pos_Left[:, j+2] - 2*R_pos_Left[:, j+1] + R_pos_Left[:, j])
+ ddr_ori_RARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_ori_Right[:, j+2] - 2*R_ori_Right[:, j+1] + R_ori_Right[:, j])
+ ddr_ori_LARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_ori_Left[:, j+2] - 2*R_ori_Left[:, j+1] + R_ori_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_right' + str(i), w_ddr * optas.sumsqr(ddr_pos_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_left' + str(i), w_ddr * optas.sumsqr(ddr_pos_LARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_ori_right' + str(i), w_ddr * optas.sumsqr(ddr_ori_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_ori_left' + str(i), w_ddr * optas.sumsqr(ddr_ori_LARM_var_MPC[:, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_middle', ddr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_right', ddr_pos_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_left', ddr_pos_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_ori_right', ddr_ori_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_ori_left', ddr_ori_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+
+ lower_acceleration = np.array([-1.5, -1.5, -0.5]); upper_acceleration = np.array([1.5, 1.5, 0.5]);
+ for i in range(self.T_MPC_planner-2):
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration-r_ep_start[6:9], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_pos_Right[:, i+2] - 2 * R_pos_Right[:, i+1] + R_pos_Right[:, i]), rhs=upper_acceleration+r_ep_start[6:9])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration-r_ep_start[9:12], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_pos_Left[:, i+2] - 2 * R_pos_Left[:, i+1] + R_pos_Left[:, i]), rhs=upper_acceleration+r_ep_start[9:12])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_pos_Right[:, i+2] - 2 * R_pos_Right[:, i+1] + R_pos_Right[:, i]), rhs=upper_acceleration)
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration, mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_pos_Left[:, i+2] - 2 * R_pos_Left[:, i+1] + R_pos_Left[:, i]), rhs=upper_acceleration)
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right1', R_pos_Right[:,8], rhs=R_pos_Right[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_right2', R_pos_Right[:,9], rhs=R_pos_Right[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left1', R_pos_Left[:,8], rhs=R_pos_Left[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_left2', R_pos_Left[:,9], rhs=R_pos_Left[:,10])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_right', R_pos_Right[:,self.T_MPC_planner-4], rhs=R_pos_Right[:,self.T_MPC_planner-1])
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_left', R_pos_Left[:,self.T_MPC_planner-4], rhs=R_pos_Left[:,self.T_MPC_planner-1])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep_start', 100*optas.sumsqr(r_ep_start))
+
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.par_msnumthreads': 14,
+# 'knitro.act_qpalg': 1,
+ 'knitro.FeasTol': 5e-5, 'knitro.OptTol': 5e-5, 'knitro.ftol':5e-5,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+
+ ### ---------------------------------------------------------
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 3000;
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+ #####################################################################################3
+ # get end-effector pose as parameters
+# pos_R = builder_wholebodyMPC.add_parameter('pos_R', 3, self.T_MPC)
+# ori_R = builder_wholebodyMPC.add_parameter('ori_R', 4, self.T_MPC)
+# pos_L = builder_wholebodyMPC.add_parameter('pos_L', 3, self.T_MPC)
+# ori_L = builder_wholebodyMPC.add_parameter('ori_L', 4, self.T_MPC)
+
+# pos_R_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+# pos_L_reasonal = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ pos_R_reasonal = builder_wholebodyMPC.add_parameter('pos_R_reasonal', 3, self.T_MPC)
+ pos_L_reasonal = builder_wholebodyMPC.add_parameter('pos_L_reasonal', 3, self.T_MPC)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+ F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ self.pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ self.ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.rotation_fnc_Right = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rotation_fnc_Left = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - 2.5*Delta_p_Right_var_MPC[:, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - 2.5*Delta_p_Left_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - 2.5*Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - 2.5*Delta_p_Left_var_MPC[:, i]))
+
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_x' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[0]-pos_R_reasonal[0, i] ))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_y' + str(i), 5*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[1]-pos_R_reasonal[1, i] - Delta_p_Right_var_MPC[1, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_z' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[2]-pos_R_reasonal[2, i] ))
+
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_x' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[0]-pos_L_reasonal[0, i] ))
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_y' + str(i), 5*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[1]-pos_L_reasonal[1, i] - Delta_p_Left_var_MPC[1, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_z' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[2]-pos_L_reasonal[2, i] ))
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Right_var_MPC[1, i] - F_ext_Right_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Left_var_MPC[1, i] - F_ext_Left_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[1, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[1, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = self.duration_MPC**2 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = self.duration_MPC**4 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+
+ #########################################################################################
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0,
+ 'print_time': 0,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5,
+ 'knitro.ftol':1e-5, 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4,
+ 'knitro.bar_penaltycons': 1, 'knitro.bar_penaltyrule':2,
+ 'knitro.bar_switchrule':2, 'knitro.linesearch': 1})
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+ self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ self.pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ self.pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ self.ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ self.ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, self.pos_Right[0], self.pos_Right[1], self.pos_Right[2], self.ori_Right[0], self.ori_Right[1], self.ori_Right[2], self.ori_Right[3],
+ self.pos_Left[0], self.pos_Left[1], self.pos_Left[2], self.ori_Left[0], self.ori_Left[1], self.ori_Left[2], self.ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self.duration_MPC_planner = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = self.pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = self.ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = self.pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = self.ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ # main execution
+ if(self._idx < self._steps):
+ time_begin = rospy.get_time()
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_pos_actual_Right = np.array(self.pos_fnc_Right_planner(self.q_curr))[0:3]
+ r_pos_actual_Left = np.array(self.pos_fnc_Left_planner(self.q_curr))[0:3]
+ dr_pos_actual_Right = np.asarray(self.pos_Jac_fnc_Right_planner(self.q_curr)) @ self.dq_curr
+ dr_pos_actual_Left = np.asarray(self.pos_Jac_fnc_Left_planner(self.q_curr)) @ self.dq_curr
+
+ r_ori_actual_Right = np.array(self.ori_fnc_Right_planner(self.q_curr))[0:4]
+ r_ori_actual_Left = np.array(self.ori_fnc_Left_planner(self.q_curr))[0:4]
+ dr_ori_actual_Right = self.angular_velocity_to_quaternionRate(r_ori_actual_Right[:, 0]) @ np.asarray(self.ori_Jac_fnc_Right_planner(self.q_curr)) @ self.dq_curr
+ dr_ori_actual_Left = self.angular_velocity_to_quaternionRate(r_ori_actual_Left[:, 0]) @ np.asarray(self.ori_Jac_fnc_Left_planner(self.q_curr)) @ self.dq_curr
+
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC planner
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ pos_R_goal = []; pos_L_goal = []; ori_R_goal = []; ori_L_goal = [];
+ _t = np.asarray(np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner))
+ for i in range(self.T_MPC_planner):
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(_t[i]).flatten()
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(_t[i]).flatten()
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_pos_Right': pos_R_goal,
+ f'R_pos_Left': pos_L_goal, f'r_ep': np.zeros(self.T_MPC_planner),
+ f'r_ep_start': np.zeros(12),
+ f'R_ori_Right': ori_R_goal, f'R_ori_Left': ori_L_goal,
+ f'r_ori_ep': np.zeros((2, self.T_MPC_planner)),
+ })
+# self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_pos_Right': np.zeros((3, self.T_MPC_planner)),
+# f'R_pos_Left': np.zeros((3, self.T_MPC_planner)), f'r_ep': np.zeros(self.T_MPC_planner) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_pos_Right': self.solution_MPC_planner[f'R_pos_Right'],
+ f'R_pos_Left': self.solution_MPC_planner[f'R_pos_Left'],
+ f'R_ori_Right': self.solution_MPC_planner[f'R_ori_Right'],
+ f'R_ori_Left': self.solution_MPC_planner[f'R_ori_Left'],
+ f'r_ori_ep': self.solution_MPC_planner[f'r_ori_ep'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'],
+ f'r_ep_start': self.solution_MPC_planner[f'r_ep_start']
+ })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': self.pos_Right, 'pos_L': self.pos_Left,
+ 'ori_R': self.ori_Right, 'ori_L': self.ori_Left,
+ 'init_r_position_Right': r_pos_actual_Right, 'init_r_position_Left': r_pos_actual_Left,
+ 'init_dr_position_Right': dr_pos_actual_Right, 'init_dr_position_Left': dr_pos_actual_Left,
+ 'init_r_orientation_Right': r_ori_actual_Right, 'init_r_orientation_Left': r_ori_actual_Left,
+ 'init_dr_orientation_Right': dr_ori_actual_Right, 'init_dr_orientation_Left': dr_ori_actual_Left,
+ 'duration_MPC_planner': self.duration_MPC_planner} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_pos_Right = np.asarray(self.solution_MPC_planner[f'R_pos_Right'])
+ R_pos_Left = np.asarray(self.solution_MPC_planner[f'R_pos_Left'])
+ R_ori_Right = np.asarray(self.solution_MPC_planner[f'R_ori_Right'])
+ R_ori_Left = np.asarray(self.solution_MPC_planner[f'R_ori_Left'])
+
+ pos_R_reasonal = np.zeros((3, self.T_MPC))
+ pos_L_reasonal = np.zeros((3, self.T_MPC))
+ ori_R_reasonal = np.zeros((4, self.T_MPC))
+ ori_L_reasonal = np.zeros((4, self.T_MPC))
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if((self.ti_MPC - self._t[self._idx-1]) <= self.duration_MPC_planner and self.duration_MPC_planner>= 2./self._freq):
+ t_nomalized = (self.ti_MPC - self._t[self._idx-1])/self.duration_MPC_planner
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Left[:, j]
+ ori_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Right[:, j]
+ ori_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Left[:, j]
+ else:
+ t_nomalized = 1.
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Left[:, j]
+ ori_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Right[:, j]
+ ori_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Left[:, j]
+# print(self._t[self._idx-1])
+# print(self.duration_MPC_planner)
+# print(R_ori_Right)
+# print(pos_R_reasonal[2, :])
+# print('left')
+# print(R_pos_Left[2, :])
+# print(pos_L_reasonal[2, :])
+ ### -----------------------------------------------------------
+ self.ti_MPC = 0
+ force_R_goal = []
+ force_L_goal = []
+# ori_R_goal = []
+# ori_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+# g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# ori_R_goal.append(g_rarm_ee_ori.tolist())
+# g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+# ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+# ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+# ori_R_goal = optas.np.array(ori_R_goal).T
+# ori_L_goal = optas.np.array(ori_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ self.G_Rotation_ee_right = self.rotation_fnc_Right(self.q_curr)
+ self.G_Rotation_ee_left = self.rotation_fnc_Left(self.q_curr)
+
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R_reasonal': pos_R_reasonal, 'pos_L_reasonal': pos_L_reasonal,
+ 'ori_R_reasonal': ori_R_reasonal, 'ori_L_reasonal': ori_L_reasonal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'inertia_Right': self.G_I_ee_r_conventional[3:6, 3:6], 'inertia_Left': self.G_I_ee_l_conventional[3:6, 3:6],
+ 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_Left_actual': self.F_ext_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box } )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ self.q_next = np.zeros(self.ndof); p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ self.q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += (1./self.duration_MPC) * self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ self.dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ self.dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+
+ self.ddq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-2):
+ self.ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[1] = np.asarray(self.pos_fnc_Right(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[1] = np.asarray(self.pos_fnc_Left(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0][1]
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., self.dq_next[2]])
+ Global_v_b = np.asarray([self.dq_next[0], self.dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+ self.duration_MPC_planner = self.duration - self._idx/self._freq
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = self.q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = self.q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = self.ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ print(rospy.get_time() - time_begin)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+ def read_right_ee_grasp_ft_data_cb(self, msg):
+ self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+ def read_left_ee_grasp_ft_data_cb(self, msg):
+ self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+ def angular_velocity_to_quaternionRate(self, quaternion):
+ A = 0.5* np.asarray([ [ quaternion[3], quaternion[2], -quaternion[1] ],
+ [ -quaternion[2], quaternion[3], quaternion[0] ],
+ [ quaternion[1], -quaternion[0], quaternion[3] ],
+ [ -quaternion[0], -quaternion[1], -quaternion[2] ]
+ ])
+# A = optas.casadi.SX(np.zeros((4, 3)))
+# A[0,0] = quaternion[3]; A[0,1] = quaternion[2]; A[0,2] = -quaternion[1];
+# A[1,0] = -quaternion[2]; A[1,1] = quaternion[3]; A[1,2] = quaternion[0];
+# A[2,0] = quaternion[1]; A[2,1] = -quaternion[0]; A[2,2] = quaternion[3];
+# A[3,0] = -quaternion[0]; A[3,1] = -quaternion[1]; A[3,2] = -quaternion[2];
+ return 0.5*A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation_notgood.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation_notgood.py
new file mode 100755
index 0000000..4eb0447
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation_notgood.py
@@ -0,0 +1,997 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+# self.dt_MPC_planner = 0.1 # time step
+ self.T_MPC_planner = 15 # T is number of time steps
+# self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_pos_Right = builder_wholebodyMPC_planner.add_decision_variables('R_pos_Right', 3, self.T_MPC_planner)
+ R_pos_Left = builder_wholebodyMPC_planner.add_decision_variables('R_pos_Left', 3, self.T_MPC_planner)
+ R_ori_Right = builder_wholebodyMPC_planner.add_decision_variables('R_ori_Right', 4, self.T_MPC_planner)
+ R_ori_Left = builder_wholebodyMPC_planner.add_decision_variables('R_ori_Left', 4, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+
+ r_ori_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ori_ep', 2, self.T_MPC_planner)
+
+ duration_MPC_planner = builder_wholebodyMPC_planner.add_parameter('duration_MPC_planner', 1)
+
+ t = np.linspace(0., 1., self.T_MPC_planner)
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+ # Add parameters
+# init_r_position_middle = builder_wholebodyMPC_planner.add_parameter('init_r_position_middle', 2)
+ init_r_position_Right = builder_wholebodyMPC_planner.add_parameter('init_r_position_Right', 3)
+ init_r_position_Left = builder_wholebodyMPC_planner.add_parameter('init_r_position_Left', 3)
+ init_r_orientation_Right = builder_wholebodyMPC_planner.add_parameter('init_r_orientation_Right', 4)
+ init_r_orientation_Left = builder_wholebodyMPC_planner.add_parameter('init_r_orientation_Left', 4)
+
+ init_dr_position_Right = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Right', 3)
+ init_dr_position_Left = builder_wholebodyMPC_planner.add_parameter('init_dr_position_Left', 3)
+ init_dr_orientation_Right = builder_wholebodyMPC_planner.add_parameter('init_dr_orientation_Right', 4)
+ init_dr_orientation_Left = builder_wholebodyMPC_planner.add_parameter('init_dr_orientation_Left', 4)
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3)
+ ori_R = builder_wholebodyMPC_planner.add_parameter('ori_R', 4)
+ ori_L = builder_wholebodyMPC_planner.add_parameter('ori_L', 4)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ r_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+# r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_pos_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_pos_Right[:, j]
+ r_pos_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_pos_Left[:, j]
+ r_ori_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_ori_Right[:, j]
+ r_ori_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_ori_Left[:, j]
+ r_middle_var_MPC[:, i] = 0.5*(r_pos_RARM_var_MPC[:, i]+r_pos_LARM_var_MPC[:, i])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ori_limit_right' + str(i), lhs=np.full((4,1), -1.), mid=R_ori_Right[:, i], rhs=np.full((4,1), 1.))
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ori_limit_left' + str(i), lhs=np.full((4,1), -1.), mid=R_ori_Left[:, i], rhs=np.full((4,1), 1.))
+ builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_right' + str(i), lhs=optas.sumsqr(R_ori_Right[:, i]), rhs=1. + r_ori_ep[0, i])
+ builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_left' + str(i), lhs=optas.sumsqr(R_ori_Left[:, i]), rhs=1. + r_ori_ep[1, i])
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality' + str(i), lhs=R_ori_Left[0, i]**2 + R_ori_Left[1, i]**2 + R_ori_Left[2, i]**2 + R_ori_Left[3, i]**2, rhs=1.)
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_z' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_z' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_equality_constraint('middle' +str(i), r_middle_var_MPC[:, i], rhs=0.5*(r_pos_RARM_var_MPC[:, i]+r_pos_LARM_var_MPC[:, i]))
+
+
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[0, i] - r_pos_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_pos_RARM_var_MPC[0, i], rhs=r_pos_LARM_var_MPC[0, i])
+
+ # optimization cost: close to target
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_x', optas.sumsqr(r_middle_var_MPC[0, -1] - 0.5*(pos_R[0] + pos_L[0])))
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_y', 10*optas.sumsqr(r_middle_var_MPC[1, -1] - 0.5*(pos_R[1] + pos_L[1])))
+# builder_wholebodyMPC_planner.add_equality_constraint('Final_middle', r_middle_var_MPC[:, self.T_MPC_planner-1], rhs=0.5*(pos_R+pos_L))
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_pos_arm', r_pos_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_pos_arm', r_pos_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_L)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_ori_arm', r_ori_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=ori_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_ori_arm', r_ori_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=ori_L)
+
+
+
+
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', optas.sumsqr(r_ep))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ori_ep', 10000*optas.sumsqr(r_ori_ep))
+
+
+ # add position constraint at the beginning state
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[:, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_pos_Right', R_pos_Right[:, 0], rhs=init_r_position_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_pos_Left', R_pos_Left[:, 0], rhs=init_r_position_Left)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_ori_Right', R_ori_Right[:, 0], rhs=init_r_orientation_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_ori_Left', R_ori_Left[:, 0], rhs=init_r_orientation_Left)
+
+ for i in range(self.T_MPC_planner):
+ if(i<(self.T_MPC_planner -1)):
+ builder_wholebodyMPC_planner.add_cost_term('Right_pos_distance' + str(i), 0.001 * optas.sumsqr(R_pos_Right[:, i+1] - R_pos_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_pos_distance' + str(i), 0.001 * optas.sumsqr(R_pos_Left[:, i+1] - R_pos_Left[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Right_ori_distance' + str(i), optas.sumsqr(R_ori_Right[:, i+1] - R_ori_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_ori_distance' + str(i), optas.sumsqr(R_ori_Left[:, i+1] - R_ori_Left[:, i]))
+ if(i<(self.T_MPC_planner -2)):
+ builder_wholebodyMPC_planner.add_cost_term('dRight_pos_distance' + str(i), 0.001 * optas.sumsqr(R_pos_Right[:, i+2]-2*R_pos_Right[:, i+1] + R_pos_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_pos_distance' + str(i), 0.001 * optas.sumsqr(R_pos_Left[:, i+2]-2*R_pos_Left[:, i+1] + R_pos_Left[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dRight_ori_distance' + str(i), optas.sumsqr(R_ori_Right[:, i+2]-2*R_ori_Right[:, i+1] + R_ori_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_ori_distance' + str(i), optas.sumsqr(R_ori_Left[:, i+2]-2*R_ori_Left[:, i+1] + R_ori_Left[:, i]))
+
+
+# dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ dr_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+# lower_velocity = np.array([-1.7, -1.7, -0.5])
+# upper_velocity = np.array([1.7, 1.7, 0.5])
+# for i in range(self.T_MPC_planner-1):
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity, mid=self.n_planner * (R_pos_Right[:, i+1] - R_pos_Right[:, i]), rhs=upper_velocity)
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity, mid=self.n_planner * (R_pos_Left[:, i+1] - R_pos_Left[:, i]), rhs=upper_velocity)
+
+
+ w_dr = 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+# dr_middle_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+ dr_pos_RARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_pos_Right[:, j+1] - R_pos_Right[:, j])
+ dr_pos_LARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_pos_Left[:, j+1] - R_pos_Left[:, j])
+ dr_ori_RARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_ori_Right[:, j+1] - R_ori_Right[:, j])
+ dr_ori_LARM_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_ori_Left[:, j+1] - R_ori_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_pos_right' + str(i), w_dr * optas.sumsqr(dr_pos_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_pos_left' + str(i), w_dr * optas.sumsqr(dr_pos_LARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_ori_right' + str(i), w_dr * optas.sumsqr(dr_ori_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_ori_left' + str(i), w_dr * optas.sumsqr(dr_ori_LARM_var_MPC[:, i]))
+
+
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_middle', dr_middle_var_MPC[:, 0], rhs=0.5*(init_dr_position_Right + init_dr_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_pos_Right', dr_pos_RARM_var_MPC[:, 0], rhs=init_dr_position_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_pos_Left', dr_pos_LARM_var_MPC[:, 0], rhs=init_dr_position_Left)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Right', dr_ori_RARM_var_MPC[:, 0], rhs=init_dr_orientation_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Left', dr_ori_LARM_var_MPC[:, 0], rhs=init_dr_orientation_Left)
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dr_middle', dr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_pos_right', dr_pos_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_pos_left', dr_pos_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_ori_right', dr_ori_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_ori_left', dr_ori_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+
+# ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ ddr_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+
+
+# lower_acceleration = np.array([-1.7, -1.7, -0.5])
+# upper_acceleration = np.array([1.7, 1.7, 0.5])
+# for i in range(self.T_MPC_planner-2):
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration, mid=self.n_planner * (self.n_planner - 1) * (R_pos_Right[:, i+2] - 2 * R_pos_Right[:, i+1] + R_pos_Right[:, i]), rhs=upper_acceleration)
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration, mid=self.n_planner * (self.n_planner - 1) * (R_pos_Left[:, i+2] - 2 * R_pos_Left[:, i+1] + R_pos_Left[:, i]), rhs=upper_acceleration)
+
+ w_ddr = 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+# ddr_middle_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+ ddr_pos_RARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_pos_Right[:, j+2] - 2*R_pos_Right[:, j+1] + R_pos_Right[:, j])
+ ddr_pos_LARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_pos_Left[:, j+2] - 2*R_pos_Left[:, j+1] + R_pos_Left[:, j])
+ ddr_ori_RARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_ori_Right[:, j+2] - 2*R_ori_Right[:, j+1] + R_ori_Right[:, j])
+ ddr_ori_LARM_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_ori_Left[:, j+2] - 2*R_ori_Left[:, j+1] + R_ori_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_pos_right' + str(i), w_ddr * optas.sumsqr(ddr_pos_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_pos_left' + str(i), w_ddr * optas.sumsqr(ddr_pos_LARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_ori_right' + str(i), w_ddr * optas.sumsqr(ddr_ori_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_ori_left' + str(i), w_ddr * optas.sumsqr(ddr_ori_LARM_var_MPC[:, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_middle', ddr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_pos_right', ddr_pos_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_pos_left', ddr_pos_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_ori_right', ddr_ori_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_ori_left', ddr_ori_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_pos_right1', R_pos_Right[:,7], rhs=R_pos_Right[:,8])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_pos_right2', R_pos_Right[:,8], rhs=R_pos_Right[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_pos_left1', R_pos_Left[:,7], rhs=R_pos_Left[:,8])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_pos_left2', R_pos_Left[:,8], rhs=R_pos_Left[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_ori_right1', R_ori_Right[:,7], rhs=R_ori_Right[:,8])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_ori_right2', R_ori_Right[:,8], rhs=R_ori_Right[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_ori_left1', R_ori_Left[:,7], rhs=R_ori_Left[:,8])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_ori_left2', R_ori_Left[:,8], rhs=R_ori_Left[:,9])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_right', R_pos_Right[:,self.T_MPC_planner-4], rhs=R_pos_Right[:,self.T_MPC_planner-1])
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_left', R_pos_Left[:,self.T_MPC_planner-4], rhs=R_pos_Left[:,self.T_MPC_planner-1])
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0,
+ 'print_time': 0,
+# 'knitro.ms_numthreads': 14,
+ 'knitro.FeasTol': 1e-4, 'knitro.OptTol': 1e-4, 'knitro.ftol':1e-4,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+
+ ### ---------------------------------------------------------
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 7 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 3000;
+
+ inertia_Right = builder_wholebodyMPC.add_parameter('inertia_Right', 3, 3) # inertia Right parameter
+ inertia_Left = builder_wholebodyMPC.add_parameter('inertia_Left', 3, 3) # inertia Left parameter
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+ #####################################################################################3
+ # get end-effector pose as parameters
+ pos_R_reasonal = builder_wholebodyMPC.add_parameter('pos_R_reasonal', 3, self.T_MPC)
+ pos_L_reasonal = builder_wholebodyMPC.add_parameter('pos_L_reasonal', 3, self.T_MPC)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+ F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.ori_Jac_fnc_Right = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_right)
+ self.ori_Jac_fnc_Left = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + inertia_Right @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + inertia_Left @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), optas.sumsqr(ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), optas.sumsqr(ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), optas.sumsqr(pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - init_Delta_position_Right - 2.5*Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), optas.sumsqr(pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - init_Delta_position_Left - 2.5*Delta_p_Left_var_MPC[:, i]))
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Right_var_MPC[1, i] - F_ext_Right_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Left_var_MPC[1, i] - F_ext_Left_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[1, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[1, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+
+ #########################################################################################
+ # setup solver
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0,
+ 'print_time': 0,
+# 'knitro.ms_numthreads': 14,
+ 'knitro.FeasTol': 1e-4, 'knitro.OptTol': 1e-4,
+ 'knitro.ftol':1e-4, 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4,
+ 'knitro.bar_penaltycons': 1, 'knitro.bar_penaltyrule':2,
+ 'knitro.bar_switchrule':2, 'knitro.linesearch': 1})
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+ self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ self.pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ self.pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ self.ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ self.ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, self.pos_Right[0], self.pos_Right[1], self.pos_Right[2], self.ori_Right[0], self.ori_Right[1], self.ori_Right[2], self.ori_Right[3],
+ self.pos_Left[0], self.pos_Left[1], self.pos_Left[2], self.ori_Left[0], self.ori_Left[1], self.ori_Left[2], self.ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self.duration_MPC_planner = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = self.pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = self.ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = self.pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = self.ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+ ### ---------------------------------------------------------
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ r_pos_actual_Right = np.array(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr))[0:3]
+ r_pos_actual_Left = np.array(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr))[0:3]
+ dr_pos_actual_Right = np.asarray(self.pos_Jac_fnc_Right(self.q_curr)) @ self.dq_curr
+ dr_pos_actual_Left = np.asarray(self.pos_Jac_fnc_Left(self.q_curr)) @ self.dq_curr
+
+ r_ori_actual_Right = np.array(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=self.q_curr))[0:4]
+ r_ori_actual_Left = np.array(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=self.q_curr))[0:4]
+ dr_ori_actual_Right = self.angular_velocity_to_quaternionRate(r_ori_actual_Right[:, 0]) @ np.asarray(self.ori_Jac_fnc_Right(self.q_curr)) @ self.dq_curr
+ dr_ori_actual_Left = self.angular_velocity_to_quaternionRate(r_ori_actual_Left[:, 0]) @ np.asarray(self.ori_Jac_fnc_Left(self.q_curr)) @ self.dq_curr
+
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC planner
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ pos_R_goal = []; pos_L_goal = []; ori_R_goal = []; ori_L_goal = [];
+ _t = np.asarray(np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner))
+ for i in range(self.T_MPC_planner):
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(_t[i]).flatten()
+ pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(_t[i]).flatten()
+ ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(_t[i]).flatten()
+ ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ pos_R_goal = optas.np.array(pos_R_goal).T
+ pos_L_goal = optas.np.array(pos_L_goal).T
+ ori_R_goal = optas.np.array(ori_R_goal).T
+ ori_L_goal = optas.np.array(ori_L_goal).T
+
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_pos_Right': pos_R_goal, f'R_pos_Left': pos_L_goal,
+ f'R_ori_Right': ori_R_goal, f'R_ori_Left': ori_L_goal,
+ f'r_ori_ep': np.zeros((2, self.T_MPC_planner)),
+ f'r_ep': np.zeros(self.T_MPC_planner) })
+# self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_pos_Right': np.zeros((3, self.T_MPC_planner)),
+# f'R_pos_Left': np.zeros((3, self.T_MPC_planner)), f'r_ep': np.zeros(self.T_MPC_planner) })
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_pos_Right': self.solution_MPC_planner[f'R_pos_Right'],
+ f'R_pos_Left': self.solution_MPC_planner[f'R_pos_Left'],
+ f'R_ori_Right': self.solution_MPC_planner[f'R_ori_Right'],
+ f'R_ori_Left': self.solution_MPC_planner[f'R_ori_Left'],
+ f'r_ori_ep': self.solution_MPC_planner[f'r_ori_ep'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'] })
+
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': self.pos_Right, 'pos_L': self.pos_Left,
+ 'ori_R': self.ori_Right, 'ori_L': self.ori_Left,
+ 'init_r_position_Right': r_pos_actual_Right, 'init_r_position_Left': r_pos_actual_Left,
+ 'init_dr_position_Right': dr_pos_actual_Right, 'init_dr_position_Left': dr_pos_actual_Left,
+ 'init_r_orientation_Right': r_ori_actual_Right, 'init_r_orientation_Left': r_ori_actual_Left,
+ 'init_dr_orientation_Right': dr_ori_actual_Right, 'init_dr_orientation_Left': dr_ori_actual_Left,
+ 'duration_MPC_planner': self.duration_MPC_planner} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_pos_Right = np.asarray(self.solution_MPC_planner[f'R_pos_Right'])
+ R_pos_Left = np.asarray(self.solution_MPC_planner[f'R_pos_Left'])
+ R_ori_Right = np.asarray(self.solution_MPC_planner[f'R_ori_Right'])
+ R_ori_Left = np.asarray(self.solution_MPC_planner[f'R_ori_Left'])
+
+# print(R_ori_Right)
+# print(R_ori_Left)
+
+# print(np.asarray(self.solution_MPC_planner[f'r_ori_ep']))
+
+ pos_R_reasonal = np.zeros((3, self.T_MPC))
+ pos_L_reasonal = np.zeros((3, self.T_MPC))
+ ori_R_reasonal = np.zeros((4, self.T_MPC))
+ ori_L_reasonal = np.zeros((4, self.T_MPC))
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if((self.ti_MPC - self._t[self._idx-1]) <= self.duration_MPC_planner and self.duration_MPC_planner > 0):
+ t_nomalized = (self.ti_MPC - self._t[self._idx-1])/self.duration_MPC_planner
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Left[:, j]
+ ori_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Right[:, j]
+ ori_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Left[:, j]
+ else:
+ pos_R_reasonal[:, i] = R_pos_Right[:, self.T_MPC_planner-1]
+ pos_L_reasonal[:, i] = R_pos_Left[:, self.T_MPC_planner-1]
+ ori_R_reasonal[:, i] = R_ori_Right[:, self.T_MPC_planner-1]
+ ori_L_reasonal[:, i] = R_ori_Left[:, self.T_MPC_planner-1]
+
+# t_nomalized = 1.
+# for j in range(self.T_MPC_planner):
+# pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Right[:, j]
+# pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Left[:, j]
+# ori_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Right[:, j]
+# ori_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Left[:, j]
+# print(self._t[self._idx-1])
+# print(self.duration_MPC_planner)
+# print(R_pos_Right[2, :])
+# print(pos_R_reasonal[2, :])
+# print('left')
+# print(R_pos_Left[2, :])
+# print(pos_L_reasonal[2, :])
+ ### -----------------------------------------------------------
+ self.ti_MPC = 0
+ force_R_goal = []
+ force_L_goal = []
+# ori_R_goal = []
+# ori_L_goal = []
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+# g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# ori_R_goal.append(g_rarm_ee_ori.tolist())
+# g_larm_ee_ori = self._LARM_ee_Quat_trajectory(self.ti_MPC).flatten()
+# ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+# ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+# ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+# ori_R_goal = optas.np.array(ori_R_goal).T
+# ori_L_goal = optas.np.array(ori_L_goal).T
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=self.q_curr)
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=self.q_curr)
+
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R_reasonal': pos_R_reasonal, 'pos_L_reasonal': pos_L_reasonal,
+ 'ori_R_reasonal': ori_R_reasonal, 'ori_L_reasonal': ori_L_reasonal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'inertia_Right': self.G_I_ee_r_conventional[3:6, 3:6], 'inertia_Left': self.G_I_ee_l_conventional[3:6, 3:6],
+ 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_Left_actual': self.F_ext_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box } )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ self.q_next = np.zeros(self.ndof); p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ self.q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+ p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ dp_right += self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ ddp_right += self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ self.dq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-1):
+ self.dq_next += self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+
+ self.ddq_next = np.zeros(self.ndof)
+ for j in range(self.T_MPC-2):
+ self.ddq_next += self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_next)).T[0][1] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[1] = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_next)).T[0][1] - np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=self.q_curr)).T[0][1]
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., self.dq_next[2]])
+ Global_v_b = np.asarray([self.dq_next[0], self.dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+# print(p_right)
+# print(self.Derivation_RARM_pos_start)
+
+ self.duration_MPC_planner = self.duration - self._idx/self._freq
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = self.q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = self.q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = self.ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+ def read_right_ee_grasp_ft_data_cb(self, msg):
+ self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+ def read_left_ee_grasp_ft_data_cb(self, msg):
+ self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+ def angular_velocity_to_quaternionRate(self, quaternion):
+ A = 0.5* np.asarray([ [ quaternion[3], quaternion[2], -quaternion[1] ],
+ [ -quaternion[2], quaternion[3], quaternion[0] ],
+ [ quaternion[1], -quaternion[0], quaternion[3] ],
+ [ -quaternion[0], -quaternion[1], -quaternion[2] ]
+ ])
+ return A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation_notgood2.py b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation_notgood2.py
new file mode 100755
index 0000000..5069ab9
--- /dev/null
+++ b/script/action_server_cmd_pose_MPC_BC_operational_pick_sensorAD_obstacle_wholetrajectory_withOrientation_notgood2.py
@@ -0,0 +1,1006 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import WrenchStamped
+from geometry_msgs.msg import Wrench
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseForceAction, CmdChonkPoseForceFeedback, CmdChonkPoseForceResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+from X_fromRandP import X_fromRandP, X_fromRandP_different
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 50)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ self.ndof = self.ndof_base + self.ndof_position_control
+ ### ---------------------------------------------------------
+ # initialize variables for planner
+ self.q_curr = np.zeros(self.ndof); self.q_curr_joint = np.zeros(self.ndof_position_control); self.q_curr_base = np.zeros(self.ndof_base);
+ self.dq_curr = np.zeros(self.ndof); self.dq_curr_joint = np.zeros(self.ndof_position_control); self.dq_curr_base = np.zeros(self.ndof_base);
+ self.joint_names_position = []; self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3'];
+ self.donkey_R =np.zeros((3,3)); self.donkey_position =np.zeros(3); self.donkey_velocity =np.zeros(3); self.donkey_angular_velocity =np.zeros(3);
+ self.q_next = np.zeros(self.ndof);
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body MPC planner in real time
+ self.wholebodyMPC_planner= optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_wholebodyMPC'
+ )
+ self.wholebodyMPC_planner_name = self.wholebodyMPC_planner.get_name()
+# self.dt_MPC_planner = 0.1 # time step
+ self.T_MPC_planner = 12 # T is number of time steps
+# self.duration_MPC_planner = float(self.T_MPC_planner-1)*self.dt_MPC_planner
+ # nominal robot configuration
+ self.wholebodyMPC_planner_opt_idx = self.wholebodyMPC_planner.optimized_joint_indexes
+ self.wholebodyMPC_planner_param_idx = self.wholebodyMPC_planner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC_planner = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC_planner])
+ builder_wholebodyMPC_planner._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC_planner._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ R_pos_Right = builder_wholebodyMPC_planner.add_decision_variables('R_pos_Right', 3, self.T_MPC_planner)
+ R_pos_Left = builder_wholebodyMPC_planner.add_decision_variables('R_pos_Left', 3, self.T_MPC_planner)
+ R_ori_Right = builder_wholebodyMPC_planner.add_decision_variables('R_ori_Right', 4, self.T_MPC_planner)
+ R_ori_Left = builder_wholebodyMPC_planner.add_decision_variables('R_ori_Left', 4, self.T_MPC_planner)
+ r_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ep', self.T_MPC_planner)
+ r_ori_ep = builder_wholebodyMPC_planner.add_decision_variables('r_ori_ep', 2, self.T_MPC_planner)
+ r_ep_start = builder_wholebodyMPC_planner.add_decision_variables('r_ep_start', 12)
+
+ duration_MPC_planner = builder_wholebodyMPC_planner.add_parameter('duration_MPC_planner', 1)
+
+ init_position_MPC_planner = builder_wholebodyMPC_planner.add_parameter('init_position_MPC_planner', self.ndof) # initial robot position
+ init_velocity_MPC_planner = builder_wholebodyMPC_planner.add_parameter('init_velocity_MPC_planner', self.ndof) # initial robot velocity
+
+ self.pos_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_position_function(link=self._link_ee_left)
+ self.ori_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_quaternion_function(link=self._link_ee_left)
+
+ self.pos_Jac_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ self.ori_Jac_fnc_Right_planner = self.wholebodyMPC_planner.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_right)
+ self.ori_Jac_fnc_Left_planner = self.wholebodyMPC_planner.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_left)
+
+ r_pos_actual_Right = optas.casadi.SX(np.zeros(3))
+ r_pos_actual_Left = optas.casadi.SX(np.zeros(3))
+ dr_pos_actual_Right = optas.casadi.SX(np.zeros(3))
+ dr_pos_actual_Left = optas.casadi.SX(np.zeros(3))
+
+ r_pos_actual_Right = self.pos_fnc_Right_planner(init_position_MPC_planner)
+ r_pos_actual_Left = self.pos_fnc_Left_planner(init_position_MPC_planner)
+ dr_pos_actual_Right = self.pos_Jac_fnc_Right_planner(init_position_MPC_planner) @ init_velocity_MPC_planner
+ dr_pos_actual_Left = self.pos_Jac_fnc_Left_planner(init_position_MPC_planner) @ init_velocity_MPC_planner
+
+ r_ori_actual_Right = optas.casadi.SX(np.zeros(4))
+ r_ori_actual_Left = optas.casadi.SX(np.zeros(4))
+ dr_ori_actual_Right = optas.casadi.SX(np.zeros(4))
+ dr_ori_actual_Left = optas.casadi.SX(np.zeros(4))
+
+ r_ori_actual_Right = self.ori_fnc_Right_planner(init_position_MPC_planner)
+ r_ori_actual_Left = self.ori_fnc_Left_planner(init_position_MPC_planner)
+ dr_ori_actual_Right = self.angular_velocity_to_quaternionRate(r_ori_actual_Right) @ self.ori_Jac_fnc_Right_planner(init_position_MPC_planner) @ init_velocity_MPC_planner
+ dr_ori_actual_Left = self.angular_velocity_to_quaternionRate(r_ori_actual_Left) @ self.ori_Jac_fnc_Left_planner(init_position_MPC_planner) @ init_velocity_MPC_planner
+
+ t = np.linspace(0., 1., self.T_MPC_planner)
+ self.n_planner = self.T_MPC_planner -1 # N in Bezier curve
+
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyMPC_planner.add_parameter('pos_R', 3)
+ pos_L = builder_wholebodyMPC_planner.add_parameter('pos_L', 3)
+ ori_R = builder_wholebodyMPC_planner.add_parameter('ori_R', 4)
+ ori_L = builder_wholebodyMPC_planner.add_parameter('ori_L', 4)
+
+ # define q function depending on P
+ r_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ r_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ r_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner):
+# r_middle_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_middle[:, j]
+ r_pos_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_pos_Right[:, j]
+ r_pos_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_pos_Left[:, j]
+ r_ori_RARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_ori_Right[:, j]
+ r_ori_LARM_var_MPC[:, i] += self.BC(self.n_planner, j) * t[i]**j * (1-t[i])**(self.n_planner-j) * R_ori_Left[:, j]
+ r_middle_var_MPC[:, i] = 0.5*(r_pos_RARM_var_MPC[:, i]+r_pos_LARM_var_MPC[:, i])
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ori_limit_right' + str(i), lhs=np.full((4,1), -1.), mid=R_ori_Right[:, i], rhs=np.full((4,1), 1.))
+# builder_wholebodyMPC_planner.add_bound_inequality_constraint('ori_limit_left' + str(i), lhs=np.full((4,1), -1.), mid=R_ori_Left[:, i], rhs=np.full((4,1), 1.))
+ builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_right' + str(i), lhs=optas.sumsqr(R_ori_Right[:, i]), rhs=1. + r_ori_ep[0, i])
+ builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality_left' + str(i), lhs=optas.sumsqr(R_ori_Left[:, i]), rhs=1. + r_ori_ep[1, i])
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('quaternion_equality' + str(i), lhs=R_ori_Left[0, i]**2 + R_ori_Left[1, i]**2 + R_ori_Left[2, i]**2 + R_ori_Left[3, i]**2, rhs=1.)
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_z' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_z' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[2, i] - r_middle_var_MPC[2, i]))
+# builder_wholebodyMPC_planner.add_equality_constraint('middle' +str(i), r_middle_var_MPC[:, i], rhs=0.5*(r_pos_RARM_var_MPC[:, i]+r_pos_LARM_var_MPC[:, i]))
+
+
+
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_x' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[0, i] - pos_R[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Right_arm_obstacle_y' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[1, i] - pos_R[1, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_x' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[0, i] - pos_L[0, i]))
+# builder_wholebodyMPC_planner.add_cost_term('Left_arm_obstacle_y' + str(i), optas.sumsqr(r_pos_LARM_var_MPC[1, i] - pos_L[1, i]))
+
+# builder_wholebodyMPC_planner.add_cost_term('mini_two_arm_pos_x_difference' + str(i), optas.sumsqr(r_pos_RARM_var_MPC[0, i] - r_pos_LARM_var_MPC[0, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('two_arm_x_position' + str(i), lhs=r_pos_RARM_var_MPC[0, i], rhs=r_pos_LARM_var_MPC[0, i])
+
+ # optimization cost: close to target
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_x', optas.sumsqr(r_middle_var_MPC[0, -1] - 0.5*(pos_R[0] + pos_L[0])))
+# builder_wholebodyMPC_planner.add_cost_term('Final_middle_y', 10*optas.sumsqr(r_middle_var_MPC[1, -1] - 0.5*(pos_R[1] + pos_L[1])))
+# builder_wholebodyMPC_planner.add_equality_constraint('Final_middle', r_middle_var_MPC[:, self.T_MPC_planner-1], rhs=0.5*(pos_R+pos_L))
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_pos_arm', r_pos_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_pos_arm', r_pos_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=pos_L)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Right_ori_arm', r_ori_RARM_var_MPC[:, self.T_MPC_planner-1], rhs=ori_R)
+ builder_wholebodyMPC_planner.add_equality_constraint('Final_Left_ori_arm', r_ori_LARM_var_MPC[:, self.T_MPC_planner-1], rhs=ori_L)
+
+
+
+
+ for i in range(self.T_MPC_planner):
+ obstacle_pos = np.asarray([[4], [0]])
+ obstacle_radius = 0.5 + 1
+ builder_wholebodyMPC_planner.add_geq_inequality_constraint('middle_obstacle' + str(i), lhs=(r_middle_var_MPC[0:2, i]-obstacle_pos).T @ (r_middle_var_MPC[0:2, i]-obstacle_pos), rhs=obstacle_radius**2 + r_ep[i])
+
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep', 10*optas.sumsqr(r_ep))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ori_ep', 10*optas.sumsqr(r_ori_ep))
+
+
+ # add position constraint at the beginning state
+# builder_wholebodyMPC_planner.add_equality_constraint('init_r_middle', R_middle[:, 0], rhs=0.5*(init_r_position_Right + init_r_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_pos_Right', R_pos_Right[:, 0], rhs=r_pos_actual_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_pos_Left', R_pos_Left[:, 0], rhs=r_pos_actual_Left)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_ori_Right', R_ori_Right[:, 0], rhs=r_ori_actual_Right)
+ builder_wholebodyMPC_planner.add_equality_constraint('init_r_ori_Left', R_ori_Left[:, 0], rhs=r_ori_actual_Left)
+
+ for i in range(self.T_MPC_planner):
+ if(i<(self.T_MPC_planner -1)):
+ builder_wholebodyMPC_planner.add_cost_term('Right_pos_distance' + str(i), 10 * optas.sumsqr(R_pos_Right[:, i+1] - R_pos_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_pos_distance' + str(i), 10 * optas.sumsqr(R_pos_Left[:, i+1] - R_pos_Left[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Right_ori_distance' + str(i), 10 * optas.sumsqr(R_ori_Right[:, i+1] - R_ori_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('Left_ori_distance' + str(i), 10 * optas.sumsqr(R_ori_Left[:, i+1] - R_ori_Left[:, i]))
+ if(i<(self.T_MPC_planner -2)):
+ builder_wholebodyMPC_planner.add_cost_term('dRight_pos_distance' + str(i), 10 * optas.sumsqr(R_pos_Right[:, i+2]-2*R_pos_Right[:, i+1] + R_pos_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_pos_distance' + str(i), 10 * optas.sumsqr(R_pos_Left[:, i+2]-2*R_pos_Left[:, i+1] + R_pos_Left[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dRight_ori_distance' + str(i), 10 * optas.sumsqr(R_ori_Right[:, i+2]-2*R_ori_Right[:, i+1] + R_ori_Right[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('dLeft_ori_distance' + str(i), 10 * optas.sumsqr(R_ori_Left[:, i+2]-2*R_ori_Left[:, i+1] + R_ori_Left[:, i]))
+
+
+# dr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ dr_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ dr_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+ w_dr = duration_MPC_planner**2 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-1):
+# dr_middle_var_MPC[:, i] += self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_middle[:, j+1] - R_middle[:, j])
+ dr_pos_RARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_pos_Right[:, j+1] - R_pos_Right[:, j])
+ dr_pos_LARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_pos_Left[:, j+1] - R_pos_Left[:, j])
+ dr_ori_RARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_ori_Right[:, j+1] - R_ori_Right[:, j])
+ dr_ori_LARM_var_MPC[:, i] += (1./duration_MPC_planner) * self.BC(self.n_planner-1, j) * t[i]**j * (1-t[i])**(self.n_planner-1-j) * self.n_planner * (R_ori_Left[:, j+1] - R_ori_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_dr_middle' + str(i), w_dr * optas.sumsqr(dr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_pos_right' + str(i), w_dr * optas.sumsqr(dr_pos_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_pos_left' + str(i), w_dr * optas.sumsqr(dr_pos_LARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_ori_right' + str(i), w_dr * optas.sumsqr(dr_ori_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_dr_ori_left' + str(i), w_dr * optas.sumsqr(dr_ori_LARM_var_MPC[:, i]))
+
+
+
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_middle', dr_middle_var_MPC[:, 0], rhs=0.5*(init_dr_position_Right + init_dr_position_Left))
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_pos_Right', dr_pos_RARM_var_MPC[0:2, 0], rhs=dr_pos_actual_Right[0:2])
+ builder_wholebodyMPC_planner.add_equality_constraint('init_dr_pos_Left', dr_pos_LARM_var_MPC[0:2, 0], rhs=dr_pos_actual_Left[0:2])
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Right', dr_ori_RARM_var_MPC[:, 0], rhs=dr_ori_actual_Right)
+# builder_wholebodyMPC_planner.add_equality_constraint('init_dr_ori_Left', dr_ori_LARM_var_MPC[:, 0], rhs=dr_ori_actual_Left)
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dr_middle', dr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_pos_right', dr_pos_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_pos_left', dr_pos_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_ori_right', dr_ori_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_dr_ori_left', dr_ori_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+
+ lower_velocity = np.array([-1., -1., -0.5]); upper_velocity = np.array([1., 1., 0.5]);
+ for i in range(self.T_MPC_planner-1):
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_right' + str(i), lhs=lower_velocity-r_ep_start[0:3], mid=(1./duration_MPC_planner) * self.n_planner * (R_pos_Right[:, i+1] - R_pos_Right[:, i]), rhs=upper_velocity+r_ep_start[0:3])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('dr_vel_limit_left' + str(i), lhs=lower_velocity-r_ep_start[3:6], mid=(1./duration_MPC_planner) * self.n_planner * (R_pos_Left[:, i+1] - R_pos_Left[:, i]), rhs=upper_velocity+r_ep_start[3:6])
+
+# ddr_middle_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_pos_RARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_pos_LARM_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC_planner)))
+ ddr_ori_RARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+ ddr_ori_LARM_var_MPC = optas.casadi.SX(np.zeros((4, self.T_MPC_planner)))
+
+ w_ddr = duration_MPC_planner**4 * 0.0005/float(self.T_MPC_planner)
+ for i in range(self.T_MPC_planner):
+ for j in range(self.T_MPC_planner-2):
+# ddr_middle_var_MPC[:, i] += self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_middle[:, j+2] - 2*R_middle[:, j+1] + R_middle[:, j])
+ ddr_pos_RARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_pos_Right[:, j+2] - 2*R_pos_Right[:, j+1] + R_pos_Right[:, j])
+ ddr_pos_LARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_pos_Left[:, j+2] - 2*R_pos_Left[:, j+1] + R_pos_Left[:, j])
+ ddr_ori_RARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_ori_Right[:, j+2] - 2*R_ori_Right[:, j+1] + R_ori_Right[:, j])
+ ddr_ori_LARM_var_MPC[:, i] += (1./duration_MPC_planner)**2 * self.BC(self.n_planner-2, j) * t[i]**j * (1-t[i])**(self.n_planner-2-j) * self.n_planner * (self.n_planner-1)* (R_ori_Left[:, j+2] - 2*R_ori_Left[:, j+1] + R_ori_Left[:, j])
+# builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_middle' + str(i), w_ddr * optas.sumsqr(ddr_middle_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_pos_right' + str(i), w_ddr * optas.sumsqr(ddr_pos_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_pos_left' + str(i), w_ddr * optas.sumsqr(ddr_pos_LARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_ori_right' + str(i), w_ddr * optas.sumsqr(ddr_ori_RARM_var_MPC[:, i]))
+ builder_wholebodyMPC_planner.add_cost_term('minimize_ddr_ori_left' + str(i), w_ddr * optas.sumsqr(ddr_ori_LARM_var_MPC[:, i]))
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_middle', ddr_middle_var_MPC[:,-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_pos_right', ddr_pos_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_pos_left', ddr_pos_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(3))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_ori_right', ddr_ori_RARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+ builder_wholebodyMPC_planner.add_equality_constraint('final_ddr_ori_left', ddr_ori_LARM_var_MPC[:,self.T_MPC_planner-1], rhs=np.zeros(4))
+
+ lower_acceleration = np.array([-1.5, -1.5, -0.5]); upper_acceleration = np.array([1.5, 1.5, 0.5]);
+ for i in range(self.T_MPC_planner-2):
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_right' + str(i), lhs=lower_acceleration-r_ep_start[6:9], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_pos_Right[:, i+2] - 2 * R_pos_Right[:, i+1] + R_pos_Right[:, i]), rhs=upper_acceleration+r_ep_start[6:9])
+ builder_wholebodyMPC_planner.add_bound_inequality_constraint('ddr_acc_limit_left' + str(i), lhs=lower_acceleration-r_ep_start[9:12], mid=(1./duration_MPC_planner)**2 * self.n_planner * (self.n_planner - 1) * (R_pos_Left[:, i+2] - 2 * R_pos_Left[:, i+1] + R_pos_Left[:, i]), rhs=upper_acceleration+r_ep_start[9:12])
+
+
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_pos_right1', R_pos_Right[:,7], rhs=R_pos_Right[:,8])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_pos_right2', R_pos_Right[:,8], rhs=R_pos_Right[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_pos_left1', R_pos_Left[:,7], rhs=R_pos_Left[:,8])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_pos_left2', R_pos_Left[:,8], rhs=R_pos_Left[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_ori_right1', R_ori_Right[:,7], rhs=R_ori_Right[:,8])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_ori_right2', R_ori_Right[:,8], rhs=R_ori_Right[:,9])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_ori_left1', R_ori_Left[:,7], rhs=R_ori_Left[:,8])
+# builder_wholebodyMPC_planner.add_equality_constraint('trajectory_middle_ddr_ori_left2', R_ori_Left[:,8], rhs=R_ori_Left[:,9])
+
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_right', R_pos_Right[:,self.T_MPC_planner-4], rhs=R_pos_Right[:,self.T_MPC_planner-1])
+# builder_wholebodyMPC_planner.add_equality_constraint('final_dddr_left', R_pos_Left[:,self.T_MPC_planner-4], rhs=R_pos_Left[:,self.T_MPC_planner-1])
+ builder_wholebodyMPC_planner.add_cost_term('minimize_r_ep_start', 100*optas.sumsqr(r_ep_start))
+ # setup solver
+ self.solver_wholebodyMPC_planner = optas.CasADiSolver(optimization=builder_wholebodyMPC_planner.build()).setup('knitro', solver_options={
+# 'knitro.OutLev': 0,
+ 'print_time': 0,
+# 'knitro.ms_numthreads': 14,
+# 'knitro.act_qpalg': 1,
+ 'knitro.FeasTol': 5e-3, 'knitro.OptTol': 5e-3, 'knitro.ftol':5e-3,
+ 'knitro.algorithm':1, 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 4.0e-3,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4, 'knitro.bar_penaltycons': 1,
+ 'knitro.bar_penaltyrule':2, 'knitro.bar_switchrule':2, 'knitro.linesearch': 1
+ } )
+ self.solution_MPC_planner = None
+
+ ### ---------------------------------------------------------
+ ### ---------------------------------------------------------
+ # set up whole-body MPC
+ wholebodyMPC_LIMITS = optas.RobotModel(urdf_string=self._robot_description, time_derivs=[0, 1], param_joints=[], name='chonk_wholebodyMPC_LIMITS')
+ self.wholebodyMPC = optas.RobotModel(
+ urdf_string=self._robot_description, time_derivs=[0],
+ param_joints=['base_joint_1', 'base_joint_2', 'base_joint_3', 'CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'], name='chonk_wholebodyMPC' )
+ lower, upper = wholebodyMPC_LIMITS.get_limits(time_deriv=0)
+ dlower, dupper = wholebodyMPC_LIMITS.get_limits(time_deriv=1)
+ self.wholebodyMPC_name = self.wholebodyMPC.get_name()
+ self.dt_MPC = 0.1 # time step
+ self.T_MPC = 6 # T is number of time steps
+ self.duration_MPC = float(self.T_MPC-1)*self.dt_MPC
+ # nominal robot configuration
+ self.wholebodyMPC_opt_idx = self.wholebodyMPC.optimized_joint_indexes
+ self.wholebodyMPC_param_idx = self.wholebodyMPC.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyMPC = optas.OptimizationBuilder(T=1, robots=[self.wholebodyMPC])
+ builder_wholebodyMPC._decision_variables = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._parameters = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_eq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._lin_ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._ineq_constraints = optas.sx_container.SXContainer()
+ builder_wholebodyMPC._eq_constraints = optas.sx_container.SXContainer()
+ # get robot state variables, get velocity state variables
+ Q = builder_wholebodyMPC.add_decision_variables('Q', self.ndof, self.T_MPC)
+ P_Right = builder_wholebodyMPC.add_decision_variables('P_Right', 3, self.T_MPC)
+ P_Left = builder_wholebodyMPC.add_decision_variables('P_Left', 3, self.T_MPC)
+
+ t = builder_wholebodyMPC.add_parameter('t', self.T_MPC) # time
+ self.n = self.T_MPC -1 # N in Bezier curve
+ # Add parameters
+ init_position_MPC = builder_wholebodyMPC.add_parameter('init_position_MPC', self.ndof) # initial robot position
+ init_velocity_MPC = builder_wholebodyMPC.add_parameter('init_velocity_MPC', self.ndof) # initial robot velocity
+ init_Delta_position_Right = builder_wholebodyMPC.add_parameter('init_Delta_position_Right', 3)
+ init_Delta_position_Left = builder_wholebodyMPC.add_parameter('init_Delta_position_Left', 3)
+ self.Derivation_RARM_pos_start = np.zeros(3)
+ self.Derivation_LARM_pos_start = np.zeros(3)
+
+ self.m_ee_r = 0.3113;
+ self.m_ee_l = 0.3113;
+ stiffness = 1000;
+
+ self.K_Right = np.diag([stiffness, stiffness, stiffness]) # Stiffness Right
+ self.K_Left = np.diag([stiffness, stiffness, stiffness]) # Stiffness Left
+ self.D_Right = np.diag([2 * np.sqrt(self.m_ee_r*self.K_Right[0,0]), 2 * np.sqrt(self.m_ee_r*self.K_Right[1,1]), 2 * np.sqrt(self.m_ee_r*self.K_Right[2,2])]) # Damping Right
+ self.D_Left = np.diag([2 * np.sqrt(self.m_ee_l*self.K_Left[0,0]), 2 * np.sqrt(self.m_ee_l*self.K_Left[1,1]), 2 * np.sqrt(self.m_ee_l*self.K_Left[2,2])]) # Damping Left
+ ###################################################################################
+ i_xx=0.00064665; i_xy=0; i_xz=0.000297068; i_yy=0.00082646; i_yz=0; i_zz=0.000354023;
+ self.sensor_p_ee_r = np.array([-0.01773, 0, 0.04772])
+ self.sensor_I_angular_ee_r = np.asarray([[i_xx, i_xy, i_xz], [i_xy, i_yy, i_yz], [i_xz, i_yz, i_zz]])
+ self.sensor_I_ee_r_conventional = np.zeros((6,6))
+ self.sensor_I_ee_r_conventional[0:3, 0:3] = self.sensor_I_angular_ee_r + self.m_ee_r * self.skew(self.sensor_p_ee_r) @ self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[0:3, 3:6] = self.m_ee_r * self.skew(self.sensor_p_ee_r)
+ self.sensor_I_ee_r_conventional[3:6, 0:3] = self.m_ee_r * self.skew(self.sensor_p_ee_r).T
+ self.sensor_I_ee_r_conventional[3:6, 3:6] = self.m_ee_r * np.identity(3)
+ self.sensor_pos_ee_Right = np.array([-0.08, 0, 0.039+0.04])
+ self.sensor_rot_ee_Right = optas.spatialmath.roty(-np.pi/2)
+ self.sensor_X_ee_r_conventional = np.zeros((6,6))
+ self.sensor_X_ee_r_conventional[0:3, 0:3] = self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 0:3] = self.skew(self.sensor_pos_ee_Right) @ self.sensor_rot_ee_Right
+ self.sensor_X_ee_r_conventional[3:6, 3:6] = self.sensor_rot_ee_Right
+ self.I_ee_r_conventional = self.sensor_X_ee_r_conventional.T @ self.sensor_I_ee_r_conventional @ self.sensor_X_ee_r_conventional
+ self.G_Rotation_ee_right = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_right, q=np.zeros(18))
+ self.G_X_ee_right = np.identity(6)
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right
+ self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T
+
+ self.sensor_p_ee_l = self.sensor_p_ee_r
+ self.sensor_I_angular_ee_l = self.sensor_I_angular_ee_r
+ self.sensor_I_ee_l_conventional = self.sensor_I_ee_r_conventional
+ self.sensor_X_ee_l_conventional = self.sensor_X_ee_r_conventional
+ self.I_ee_l_conventional = self.sensor_X_ee_l_conventional.T @ self.sensor_I_ee_l_conventional @ self.sensor_X_ee_l_conventional
+ self.G_Rotation_ee_left = self.wholebodyMPC.get_global_link_rotation(link=self._link_ee_left, q=np.zeros(18))
+ self.G_X_ee_left = np.identity(6)
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left
+ self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T
+
+ self.rotation_fnc_Right = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_right)
+ self.rotation_fnc_Left = self.wholebodyMPC.get_global_link_rotation_function(link=self._link_ee_left)
+
+ self.G_Rotation_ee_right = optas.casadi.SX(np.zeros((3, 3)))
+ self.G_Rotation_ee_right = self.rotation_fnc_Right(init_position_MPC)
+ self.G_Rotation_ee_left = optas.casadi.SX(np.zeros((3, 3)))
+ self.G_Rotation_ee_left = self.rotation_fnc_Left(init_position_MPC)
+
+ self.G_X_ee_right = optas.casadi.SX(np.zeros((6, 6)))
+ self.G_X_ee_right[0:3, 0:3] = self.G_Rotation_ee_right; self.G_X_ee_right[3:6, 3:6] = self.G_Rotation_ee_right;
+ self.G_X_ee_left = optas.casadi.SX(np.zeros((6, 6)))
+ self.G_X_ee_left[0:3, 0:3] = self.G_Rotation_ee_left; self.G_X_ee_left[3:6, 3:6] = self.G_Rotation_ee_left;
+
+ self.G_I_ee_r_conventional = optas.casadi.SX(np.zeros((6, 6)))
+ self.G_I_ee_r_conventional = self.G_X_ee_right @ self.I_ee_r_conventional @ self.G_X_ee_right.T;
+ self.G_I_ee_l_conventional = optas.casadi.SX(np.zeros((6, 6)))
+ self.G_I_ee_l_conventional = self.G_X_ee_left @ self.I_ee_l_conventional @ self.G_X_ee_left.T;
+ #####################################################################################3
+ # get end-effector pose as parameters
+ pos_R_reasonal = builder_wholebodyMPC.add_parameter('pos_R_reasonal', 3, self.T_MPC)
+ pos_L_reasonal = builder_wholebodyMPC.add_parameter('pos_L_reasonal', 3, self.T_MPC)
+ ori_R_reasonal = builder_wholebodyMPC.add_parameter('ori_R_reasonal', 4, self.T_MPC)
+ ori_L_reasonal = builder_wholebodyMPC.add_parameter('ori_L_reasonal', 4, self.T_MPC)
+
+ ddpos_box_goal = builder_wholebodyMPC.add_parameter('ddpos_box_goal', 3, self.T_MPC)
+ m_box = builder_wholebodyMPC.add_parameter('m_box', 1)
+ #####################################################################################
+ self.F_ext_Right = np.zeros(6); self.F_ext_Left = np.zeros(6);
+ self.acc_box = np.zeros((3, self.T_MPC));
+# self.acc_box = np.zeros(3);
+ F_ext_Right_goal = builder_wholebodyMPC.add_parameter('F_ext_Right_goal', 3, self.T_MPC)
+ F_ext_Left_goal = builder_wholebodyMPC.add_parameter('F_ext_Left_goal', 3, self.T_MPC)
+ F_ext_Right_actual = builder_wholebodyMPC.add_parameter('F_ext_Right_actual', 3)
+ F_ext_Left_actual = builder_wholebodyMPC.add_parameter('F_ext_Left_actual', 3)
+ F_ext_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ F_ext_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ # functions of right and left arm positions
+ self.pos_fnc_Right = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_right)
+ self.pos_fnc_Left = self.wholebodyMPC.get_global_link_position_function(link=self._link_ee_left)
+ self.pos_Jac_fnc_Right = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.pos_Jac_fnc_Left = self.wholebodyMPC.get_global_link_linear_jacobian_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ self.ori_fnc_Right = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_right)
+ self.ori_fnc_Left = self.wholebodyMPC.get_global_link_quaternion_function(link=self._link_ee_left)
+ self.ori_Jac_fnc_Right = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_right)
+ self.ori_Jac_fnc_Left = self.wholebodyMPC.get_global_link_angular_geometric_jacobian_function(link=self._link_ee_left)
+ #####################################################################################
+ # define q function depending on P
+ q_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ #####################################################################################
+ Delta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ Delta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ dDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Right_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ ddDelta_p_Left_var_MPC = optas.casadi.SX(np.zeros((3, self.T_MPC)))
+ #####################################################################################
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC[:, i] += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * Q[:, j]
+ Delta_p_Right_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Right[:, j]
+ Delta_p_Left_var_MPC += self.BC(self.n, j) * t[i]**j * (1-t[i])**(self.n-j) * P_Left[:, j]
+ for j in range(self.T_MPC-1):
+ dDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ dDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (P_Left[:, j+1] - P_Left[:, j])
+ for j in range(self.T_MPC-2):
+ ddDelta_p_Right_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+ ddDelta_p_Left_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (P_Left[:, j+2] - 2*P_Left[:, j+1] + P_Left[:, j])
+ #####################################################################################
+ F_ext_Right_var_MPC[:, i] = F_ext_Right_actual + self.G_I_ee_r_conventional[3:6, 3:6] @ ddDelta_p_Right_var_MPC[:, i] + self.K_Right @ Delta_p_Right_var_MPC[:, i] + self.D_Right @ dDelta_p_Right_var_MPC[:, i]
+ F_ext_Left_var_MPC[:, i] = F_ext_Left_actual + self.G_I_ee_l_conventional[3:6, 3:6] @ ddDelta_p_Left_var_MPC[:, i] + self.K_Left @ Delta_p_Left_var_MPC[:, i] + self.D_Left @ dDelta_p_Left_var_MPC[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint('control_point_' + str(i) + '_bound', lhs=lower, mid=Q[:, i], rhs=upper)
+ # optimization cost: close to target
+ builder_wholebodyMPC.add_cost_term('Right_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Right(q_var_MPC[:, i])-ori_R_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm orientation' + str(i), 10*optas.sumsqr(self.ori_fnc_Left(q_var_MPC[:, i])-ori_L_reasonal[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])-pos_R_reasonal[:, i] - 2.5*Delta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm position AD' + str(i), 3*optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])-pos_L_reasonal[:, i] - 2.5*Delta_p_Left_var_MPC[:, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_x' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[0]-pos_R_reasonal[0, i] ))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_y' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[1]-pos_R_reasonal[1, i] - init_Delta_position_Right[1] - Delta_p_Right_var_MPC[1, i]))
+# builder_wholebodyMPC.add_cost_term('Right_arm position AD_z' + str(i), optas.sumsqr(self.pos_fnc_Right(q_var_MPC[:, i])[2]-pos_R_reasonal[2, i] ))
+
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_x' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[0]-pos_L_reasonal[0, i] ))
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_y' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[1]-pos_L_reasonal[1, i] - init_Delta_position_Left[1] - Delta_p_Left_var_MPC[1, i]))
+# builder_wholebodyMPC.add_cost_term('Left_arm position AD_z' + str(i), optas.sumsqr(self.pos_fnc_Left(q_var_MPC[:, i])[2]-pos_L_reasonal[2, i] ))
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('Right_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Right_var_MPC[1, i] - F_ext_Right_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+ builder_wholebodyMPC.add_cost_term('Left_arm Force world y' + str(i), 0.1*optas.sumsqr(F_ext_Left_var_MPC[1, i] - F_ext_Left_goal[1, i] - 0.5*m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_cost_term('two Force sum world y' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - F_ext_Right_goal[1, i] - F_ext_Left_goal[1, i] ))
+# builder_wholebodyMPC.add_cost_term('Two arm ee addition motion equal' + str(i), optas.sumsqr(Delta_p_Right_var_MPC[1, i] + Delta_p_Left_var_MPC[1, i]))
+ builder_wholebodyMPC.add_cost_term('Two force Delta for box inertial force' + str(i), optas.sumsqr(F_ext_Right_var_MPC[1, i] + F_ext_Left_var_MPC[1, i] - m_box * ddpos_box_goal[1, i]))
+# builder_wholebodyMPC.add_bound_inequality_constraint('right_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Right_var_MPC[1, i], rhs=50)
+# builder_wholebodyMPC.add_bound_inequality_constraint('left_force_limit' + str(i) + '_bound', lhs=-50, mid=F_ext_Left_var_MPC[1, i], rhs=50)
+ #####################################################################################
+ builder_wholebodyMPC.add_cost_term('twoarm_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[6, i] + q_var_MPC[12, i]))
+ builder_wholebodyMPC.add_cost_term('chest_miniscope' + str(i), optas.sumsqr(q_var_MPC[3, i]))
+ builder_wholebodyMPC.add_cost_term('arm_joint_miniscope' + str(i), 0.001 * optas.sumsqr(q_var_MPC[6:self.ndof, i]))
+ builder_wholebodyMPC.add_cost_term('donkey_yaw_miniscope' + str(i), 0.1 * optas.sumsqr(q_var_MPC[2, i]))
+ if(i<(self.T_MPC -1)):
+ builder_wholebodyMPC.add_cost_term('joint_distance' + str(i), 0.05 * optas.sumsqr(Q[:, i+1] - Q[:, i]))
+ builder_wholebodyMPC.add_cost_term('Right_force_distance' + str(i), 0.05 * optas.sumsqr(P_Right[:, i+1] - P_Right[:, i]))
+ builder_wholebodyMPC.add_cost_term('Left_force_distance' + str(i), 0.05 * optas.sumsqr(P_Left[:, i+1] - P_Left[:, i]))
+ #########################################################################################
+ # add position constraint at the beginning state
+ builder_wholebodyMPC.add_equality_constraint('init_position', Q[0:4, 0], rhs=init_position_MPC[0:4])
+ builder_wholebodyMPC.add_equality_constraint('init_position2', Q[6:self.ndof, 0], rhs=init_position_MPC[6:self.ndof])
+ builder_wholebodyMPC.add_equality_constraint('head_miniscope', Q[4:6, :], rhs=np.zeros((2, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_x', P_Right[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Right_var_MPC_non_motion_direction_z', P_Right[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_x', P_Left[0, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('Delta_p_Left_var_MPC_non_motion_direction_z', P_Left[2, :], rhs=np.zeros((1, self.T_MPC)))
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Right_constraint_y', P_Right[1, 0], rhs = 0 )
+ builder_wholebodyMPC.add_equality_constraint('init_Delta_position_Left_constraint_y', P_Left[1, 0], rhs = 0 )
+ #########################################################################################
+ dq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_dq = self.duration_MPC**2 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-1):
+ dq_var_MPC[:, i] += (1./self.duration_MPC) * self.BC(self.n-1, j) * t[i]**j * (1-t[i])**(self.n-1-j) * self.n * (Q[:, j+1] - Q[:, j])
+ if(i<(self.T_MPC -1)):
+ name = 'control_point_deriv_' + str(i) + '_bound' # add velocity constraint for each Q[:, i]
+ builder_wholebodyMPC.add_bound_inequality_constraint(name, lhs=dlower, mid=(1./self.duration_MPC) * self.n * (Q[:, i+1] - Q[:, i]), rhs=dupper)
+ builder_wholebodyMPC.add_cost_term('minimize_velocity' + str(i), w_dq * optas.sumsqr(dq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Right' + str(i), w_dq * optas.sumsqr(dDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_dDelta_p_Left' + str(i), w_dq * optas.sumsqr(dDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ ddq_var_MPC = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ w_ddq = self.duration_MPC**4 * 0.05/float(self.T_MPC)
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t[i]**j * (1-t[i])**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ builder_wholebodyMPC.add_cost_term('minimize_acceleration' + str(i), w_ddq * optas.sumsqr(ddq_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Right' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Right_var_MPC[:, i]))
+ builder_wholebodyMPC.add_cost_term('minimize_ddDelta_p_Left' + str(i), w_ddq * optas.sumsqr(ddDelta_p_Left_var_MPC[:, i]))
+ #########################################################################################
+ acc_box_var = builder_wholebodyMPC.add_decision_variables('acc_box_var', 3, self.T_MPC)
+ q_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ ddq_var_MPC_for_box = optas.casadi.SX(np.zeros((self.ndof, self.T_MPC)))
+ t_loop = (1./self._freq)/self.duration_MPC
+ for i in range(self.T_MPC):
+ for j in range(self.T_MPC):
+ q_var_MPC_for_box[:, i] += self.BC(self.n, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-j) * Q[:, j]
+ for j in range(self.T_MPC-2):
+ ddq_var_MPC_for_box[:, i] += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * (t[i]+t_loop)**j * (1-t[i]-t_loop)**(self.n-2-j) * self.n * (self.n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+ acc_box_var[:, i] = 0.5*(self.pos_Jac_fnc_Right(q_var_MPC_for_box[:, i]) + self.pos_Jac_fnc_Left(q_var_MPC_for_box[:, i])) @ ddq_var_MPC_for_box[:, i]
+
+ #########################################################################################
+ # setup solver
+# self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro')
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 10} )
+ # self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={'knitro.OutLev': 0, 'print_time': 0} )
+ self.solver_wholebodyMPC = optas.CasADiSolver(optimization=builder_wholebodyMPC.build()).setup('knitro', solver_options={
+ 'knitro.OutLev': 0,
+ 'print_time': 0,
+# 'knitro.ms_numthreads': 14,
+ 'knitro.FeasTol': 1e-5, 'knitro.OptTol': 1e-5,
+ 'knitro.ftol':1e-5, 'knitro.algorithm':1,
+ 'knitro.linsolver':2,
+# 'knitro.maxtime_real': 1.8e-2,
+ 'knitro.bar_initpt':3, 'knitro.bar_murule':4,
+ 'knitro.bar_penaltycons': 1, 'knitro.bar_penaltyrule':2,
+ 'knitro.bar_switchrule':2, 'knitro.linesearch': 1})
+ #########################################################################################
+ self.solution_MPC = None
+ self.time_linspace = np.linspace(0., self.duration_MPC, self.T_MPC)
+ self.timebyT = np.asarray(self.time_linspace)/self.duration_MPC
+
+ self.start_RARM_force = np.zeros(3); self.start_RARM_torque = np.zeros(3);
+ self.start_LARM_force = np.zeros(3); self.start_LARM_torque = np.zeros(3);
+ #########################################################################################
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ # initialize the message
+ self._msg_acceleration = Float64MultiArray()
+ self._msg_acceleration.layout = MultiArrayLayout()
+ self._msg_acceleration.layout.data_offset = 0
+ self._msg_acceleration.layout.dim.append(MultiArrayDimension())
+ self._msg_acceleration.layout.dim[0].label = "columns"
+ self._msg_acceleration.layout.dim[0].size = self.ndof
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0; self._msg_velocity.linear.y = 0; self._msg_velocity.linear.z = 0;
+ self._msg_velocity.angular.x = 0; self._msg_velocity.angular.y = 0; self._msg_velocity.angular.z = 0;
+ #########################################################################################
+ self.eva_trajectory = JointTrajectory()
+ self.eva_trajectory.header.frame_id = ''
+ self.eva_trajectory.joint_names = ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1',
+ 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5',
+ 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
+ self.eva_point = JointTrajectoryPoint()
+ self.eva_point.time_from_start = rospy.Duration(0.1)
+ self.eva_trajectory.points.append(self.eva_point)
+ ### ---------------------------------------------------------
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber("/chonk/joint_states", JointState, self.read_joint_states_cb)
+# self._joint_sub_base = rospy.Subscriber("/chonk/donkey_velocity_controller/odom", Odometry, self.read_base_states_cb)
+ self._joint_sub_base = rospy.Subscriber("/chonk/base_pose_ground_truth", Odometry, self.read_base_states_cb)
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher("/chonk/trajectory_controller/command", JointTrajectory, queue_size=10)
+ # declare acceleration publisher for two arms
+ self._joint_acc_pub = rospy.Publisher("/chonk/joint_acc_pub", Float64MultiArray, queue_size=10)
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher("/chonk/donkey_velocity_controller/cmd_vel", Twist, queue_size=10)
+ # declare two arm ee grasp actual force publisher
+ self._sensor_ft_sub_right = rospy.Subscriber("/chonk/sensor_ft_right", Float64MultiArray, self.read_right_ee_grasp_ft_data_cb)
+ self._sensor_ft_sub_left = rospy.Subscriber("/chonk/sensor_ft_left", Float64MultiArray, self.read_left_ee_grasp_ft_data_cb)
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber("/mux_selected", String, self.read_mux_selection)
+ # initialize action messages
+ self._feedback = CmdChonkPoseForceFeedback()
+ self._result = CmdChonkPoseForceResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer('cmd_pose', CmdChonkPoseForceAction, execute_cb=None, auto_start=False)
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ self.m_box = acceped_goal.m_box
+ self.pos_Right = np.asarray([acceped_goal.poseR.position.x, acceped_goal.poseR.position.y, acceped_goal.poseR.position.z])
+ self.pos_Left = np.asarray([acceped_goal.poseL.position.x, acceped_goal.poseL.position.y, acceped_goal.poseL.position.z])
+ self.ori_Right = np.asarray([acceped_goal.poseR.orientation.x, acceped_goal.poseR.orientation.y, acceped_goal.poseR.orientation.z, acceped_goal.poseR.orientation.w])
+ self.ori_Left = np.asarray([acceped_goal.poseL.orientation.x, acceped_goal.poseL.orientation.y, acceped_goal.poseL.orientation.z, acceped_goal.poseL.orientation.w])
+ self.force_Right = np.asarray([acceped_goal.ForceTorqueR.force.x, acceped_goal.ForceTorqueR.force.y, acceped_goal.ForceTorqueR.force.z])
+ self.torque_Right = np.asarray([acceped_goal.ForceTorqueR.torque.x, acceped_goal.ForceTorqueR.torque.y, acceped_goal.ForceTorqueR.torque.z])
+ self.force_Left = np.asarray([acceped_goal.ForceTorqueL.force.x, acceped_goal.ForceTorqueL.force.y, acceped_goal.ForceTorqueL.force.z])
+ self.torque_Left = np.asarray([acceped_goal.ForceTorqueL.torque.x, acceped_goal.ForceTorqueL.torque.y, acceped_goal.ForceTorqueL.torque.z])
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name, self.pos_Right[0], self.pos_Right[1], self.pos_Right[2], self.ori_Right[0], self.ori_Right[1], self.ori_Right[2], self.ori_Right[3],
+ self.pos_Left[0], self.pos_Left[1], self.pos_Left[2], self.ori_Left[0], self.ori_Left[1], self.ori_Left[2], self.ori_Left[3], acceped_goal.duration))
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ qT = np.zeros(self.ndof)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ ### ---------------------------------------------------------
+ # get two-arm end effector trajectory in the operational space
+ q0 = self.q_curr.T
+ self.duration = acceped_goal.duration
+ self.duration_MPC_planner = acceped_goal.duration
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ # current right and left arm end effector position and quaternion
+ self.start_RARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_right, q=q0)).T[0]
+ self.start_RARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_right, q=q0)).T[0]
+ self.start_LARM_quat = np.asarray(self.wholebodyMPC.get_global_link_quaternion(link=self._link_ee_left, q=q0)).T[0]
+ self.start_LARM_pos = np.asarray(self.wholebodyMPC.get_global_link_position(link=self._link_ee_left, q=q0)).T[0]
+ # derivation of right and left arm end effector position and quaternion compared with the beginning ee position and quaternion
+ Derivation_RARM_Pos = self.pos_Right - self.start_RARM_pos; Derivation_RARM_Quat = self.ori_Right - self.start_RARM_quat;
+ Derivation_LARM_Pos = self.pos_Left - self.start_LARM_pos; Derivation_LARM_Quat = self.ori_Left - self.start_LARM_quat;
+ Derivation_RARM_force = self.force_Right - self.start_RARM_force; Derivation_RARM_torque = self.torque_Right - self.start_RARM_torque;
+ Derivation_LARM_force = self.force_Left - self.start_LARM_force; Derivation_LARM_torque = self.torque_Left - self.start_LARM_torque;
+ # interpolate between current and target position polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Pos_trajectory = lambda t: self.start_RARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_Pos_trajectory = lambda t: self.start_LARM_pos + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Pos # 5th order
+ self._RARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_RARM_Pos # 5th order
+ self._LARM_ee_ddPos_trajectory = lambda t: (60.*((t/self.duration)/(self.duration**2)) - 180.*((t/self.duration)**2/(self.duration**2)) + 120.*((t/self.duration)**3/(self.duration**2)))*Derivation_LARM_Pos # 5th order
+ # interpolate between current and target quaternion polynomial obtained for zero speed (3rd order) and acceleratin (5th order) at the initial and final time
+ self._RARM_ee_Quat_trajectory = lambda t: self.start_RARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_Quat # 5th order
+ self._LARM_ee_Quat_trajectory = lambda t: self.start_LARM_quat + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_Quat # 5th order
+ # interpolate between zero and target force polynomail obtained for zero speed (3rd order) and acceleration (5th order) at the initial and final time
+ self._RARM_ee_force_trajectory = lambda t: self.start_RARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_force # 5th order
+ self._RARM_ee_torque_trajectory = lambda t: self.start_RARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_RARM_torque # 5th order
+ self._LARM_ee_force_trajectory = lambda t: self.start_LARM_force + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_force # 5th order
+ self._LARM_ee_torque_trajectory = lambda t: self.start_LARM_torque + (10.*((t/self.duration)**3) - 15.*((t/self.duration)**4) + 6.*((t/self.duration)**5))*Derivation_LARM_torque # 5th order
+
+ self._t = np.linspace(0., self.duration, self._steps + 1)
+
+ self.pos_R_goal = []; self.pos_L_goal = []; self.ori_R_goal = []; self.ori_L_goal = [];
+ _t = np.asarray(np.linspace(0., self.duration_MPC_planner, self.T_MPC_planner))
+ for i in range(self.T_MPC_planner):
+ try:
+ g_rarm_ee_pos = self._RARM_ee_Pos_trajectory(_t[i]).flatten()
+ self.pos_R_goal.append(g_rarm_ee_pos.tolist())
+ g_larm_ee_pos = self._LARM_ee_Pos_trajectory(_t[i]).flatten()
+ self.pos_L_goal.append(g_larm_ee_pos.tolist())
+ g_rarm_ee_ori = self._RARM_ee_Quat_trajectory(_t[i]).flatten()
+ self.ori_R_goal.append(g_rarm_ee_ori.tolist())
+ g_larm_ee_ori = self._LARM_ee_Quat_trajectory(_t[i]).flatten()
+ self.ori_L_goal.append(g_larm_ee_ori.tolist())
+ except ValueError:
+ self.pos_R_goal.append(g_rarm_ee_pos.tolist()) # i.e. previous goal
+ self.pos_L_goal.append(g_larm_ee_pos.tolist()) # i.e. previous goal
+ self.ori_R_goal.append(g_rarm_ee_ori.tolist()) # i.e. previous goal
+ self.ori_L_goal.append(g_larm_ee_ori.tolist()) # i.e. previous goal
+ self.pos_R_goal = optas.np.array(self.pos_R_goal).T
+ self.pos_L_goal = optas.np.array(self.pos_L_goal).T
+ self.ori_R_goal = optas.np.array(self.ori_R_goal).T
+ self.ori_L_goal = optas.np.array(self.ori_L_goal).T
+ ### ---------------------------------------------------------
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ self.curr_MPC = np.zeros((self.ndof, self.T_MPC))
+ for i in range(self.T_MPC):
+ self.curr_MPC[:,i] = self.q_curr
+
+ def timer_cb(self, event):
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ # main execution
+ if(self._idx < self._steps):
+ time_begin = rospy.get_time()
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+ ### -----------------------------------------------------------
+ ### optas. Solve the whole-body MPC planner
+ # set initial seed
+ if self.solution_MPC_planner is None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_pos_Right': self.pos_R_goal, f'R_pos_Left': self.pos_L_goal,
+ f'R_ori_Right': self.ori_R_goal, f'R_ori_Left': self.ori_L_goal,
+ f'r_ori_ep': np.zeros((2, self.T_MPC_planner)),
+ f'r_ep': np.zeros(self.T_MPC_planner),
+ f'r_ep_start': np.zeros(12)})
+ # set initial seed
+ if self.solution_MPC_planner is not None:
+ self.solver_wholebodyMPC_planner.reset_initial_seed({f'R_pos_Right': self.solution_MPC_planner[f'R_pos_Right'],
+ f'R_pos_Left': self.solution_MPC_planner[f'R_pos_Left'],
+ f'R_ori_Right': self.solution_MPC_planner[f'R_ori_Right'],
+ f'R_ori_Left': self.solution_MPC_planner[f'R_ori_Left'],
+ f'r_ori_ep': self.solution_MPC_planner[f'r_ori_ep'],
+ f'r_ep': self.solution_MPC_planner[f'r_ep'],
+ f'r_ep_start': self.solution_MPC_planner[f'r_ep_start']})
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ self.solver_wholebodyMPC_planner.reset_parameters({'pos_R': self.pos_Right, 'pos_L': self.pos_Left,
+ 'ori_R': self.ori_Right, 'ori_L': self.ori_Left,
+ 'init_position_MPC_planner': self.q_curr,
+ 'init_velocity_MPC_planner': self.dq_curr,
+ 'duration_MPC_planner': self.duration_MPC_planner} )
+ # solve problem
+ self.solution_MPC_planner = self.solver_wholebodyMPC_planner.opt.decision_variables.vec2dict(self.solver_wholebodyMPC_planner._solve())
+ R_pos_Right = np.asarray(self.solution_MPC_planner[f'R_pos_Right'])
+ R_pos_Left = np.asarray(self.solution_MPC_planner[f'R_pos_Left'])
+ R_ori_Right = np.asarray(self.solution_MPC_planner[f'R_ori_Right'])
+ R_ori_Left = np.asarray(self.solution_MPC_planner[f'R_ori_Left'])
+
+# print(R_pos_Right)
+# print(R_pos_Left)
+# print(np.asarray(self.solution_MPC_planner[f'r_ori_ep']))
+
+ pos_R_reasonal = np.zeros((3, self.T_MPC))
+ pos_L_reasonal = np.zeros((3, self.T_MPC))
+ ori_R_reasonal = np.zeros((4, self.T_MPC))
+ ori_L_reasonal = np.zeros((4, self.T_MPC))
+
+ for i in range(self.T_MPC):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if((self.ti_MPC - self._t[self._idx-1]) <= self.duration_MPC_planner and self.duration_MPC_planner > 2./self._freq):
+ t_nomalized = (self.ti_MPC - self._t[self._idx-1])/self.duration_MPC_planner
+ for j in range(self.T_MPC_planner):
+ pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Right[:, j]
+ pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Left[:, j]
+ ori_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Right[:, j]
+ ori_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Left[:, j]
+ else:
+ pos_R_reasonal[:, i] = R_pos_Right[:, self.T_MPC_planner-1]
+ pos_L_reasonal[:, i] = R_pos_Left[:, self.T_MPC_planner-1]
+ ori_R_reasonal[:, i] = R_ori_Right[:, self.T_MPC_planner-1]
+ ori_L_reasonal[:, i] = R_ori_Left[:, self.T_MPC_planner-1]
+
+# t_nomalized = 1.
+# for j in range(self.T_MPC_planner):
+# pos_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Right[:, j]
+# pos_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_pos_Left[:, j]
+# ori_R_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Right[:, j]
+# ori_L_reasonal[:, i] += self.BC(self.n_planner, j) * (t_nomalized)**j * (1-t_nomalized)**(self.n_planner-j) * R_ori_Left[:, j]
+# print(self._t[self._idx-1])
+# print(self.duration_MPC_planner)
+# print(R_pos_Right[2, :])
+# print(pos_R_reasonal[2, :])
+# print('left')
+# print(R_pos_Left[2, :])
+# print(pos_L_reasonal[2, :])
+ ### -----------------------------------------------------------
+
+ self.ti_MPC = 0; force_R_goal = []; force_L_goal = [];
+ for i in range(self.T_MPC):
+ if(self.ti_MPC <= self.duration):
+ self.ti_MPC = self._t[self._idx-1] + self.dt_MPC*i
+ if(self.ti_MPC > self.duration):
+ self.ti_MPC = self.duration
+ try:
+ g_rarm_ee_force = self._RARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_R_goal.append(g_rarm_ee_force.tolist())
+ g_larm_ee_force = self._LARM_ee_force_trajectory(self.ti_MPC).flatten()
+ force_L_goal.append(g_larm_ee_force.tolist())
+ except ValueError:
+ force_R_goal.append(g_rarm_ee_force.tolist()) # i.e. previous goal
+ force_L_goal.append(g_larm_ee_force.tolist()) # i.e. previous goal
+
+ force_R_goal = optas.np.array(force_R_goal).T
+ force_L_goal = optas.np.array(force_L_goal).T
+ ### ---------------------------------------------------------
+ ### optas. Solve the whole-body MPC
+ # set initial seed
+ if self.solution_MPC is None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.curr_MPC, f'P_Right': np.zeros((3, self.T_MPC)),
+ f'P_Left': np.zeros((3, self.T_MPC)), f'acc_box_var': np.zeros((3, self.T_MPC))})
+ # set initial seed
+ if self.solution_MPC is not None:
+ self.solver_wholebodyMPC.reset_initial_seed({f'Q': self.solution_MPC[f'Q'], f'P_Right': self.solution_MPC[f'P_Right'],
+ f'P_Left': self.solution_MPC[f'P_Left'], f'acc_box_var': self.solution_MPC[f'acc_box_var'] })
+
+ # read current robot joint positions
+# self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+# self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ self.Derivation_RARM_pos_start[1] = np.asarray(self.pos_fnc_Right(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Right(self.q_curr)).T[0][1]
+ self.Derivation_LARM_pos_start[1] = np.asarray(self.pos_fnc_Left(self.q_next)).T[0][1] - np.asarray(self.pos_fnc_Left(self.q_curr)).T[0][1]
+
+ self.solver_wholebodyMPC.reset_parameters({'pos_R_reasonal': pos_R_reasonal, 'pos_L_reasonal': pos_L_reasonal,
+ 'ori_R_reasonal': ori_R_reasonal, 'ori_L_reasonal': ori_L_reasonal,
+ 't': self.timebyT, 'init_position_MPC': self.q_curr, 'init_velocity_MPC': self.dq_curr,
+ 'F_ext_Right_goal': force_R_goal, 'F_ext_Left_goal': force_L_goal,
+ 'F_ext_Right_actual': self.F_ext_Right[3:6], 'F_ext_Left_actual': self.F_ext_Left[3:6],
+ 'init_Delta_position_Right': self.Derivation_RARM_pos_start, 'init_Delta_position_Left': self.Derivation_LARM_pos_start,
+ 'ddpos_box_goal': self.acc_box, 'm_box': self.m_box } )
+
+ # solve problem
+ self.solution_MPC = self.solver_wholebodyMPC.opt.decision_variables.vec2dict(self.solver_wholebodyMPC._solve())
+ Q = np.asarray(self.solution_MPC[f'Q'])
+ P_Right = np.asarray(self.solution_MPC[f'P_Right'])
+ P_Left = np.asarray(self.solution_MPC[f'P_Left'])
+ self.acc_box = np.asarray(self.solution_MPC[f'acc_box_var'])
+ ### ---------------------------------------------------------
+ # compute next configuration with lambda function
+ t = (1./self._freq)/self.duration_MPC; n = self.T_MPC -1;
+ self.q_next = np.zeros(self.ndof); self.dq_next = np.zeros(self.ndof); self.ddq_next = np.zeros(self.ndof);
+ p_right = np.zeros(3); dp_right = np.zeros(3); ddp_right = np.zeros(3);
+ for j in range(self.T_MPC):
+ self.q_next += self.BC(n, j) * t**j * (1-t)**(n-j) * Q[:, j]
+# p_right += self.BC(n, j) * t**j * (1-t)**(n-j) * P_Right[:, j]
+ for j in range(self.T_MPC-1):
+ self.dq_next += (1./self.duration_MPC) * self.BC(n-1, j) * t**j * (1-t)**(n-1-j) * n * (Q[:, j+1] - Q[:, j])
+# dp_right += (1./self.duration_MPC) * self.BC(self.n-1, j) * t**j * (1-t)**(self.n-1-j) * self.n * (P_Right[:, j+1] - P_Right[:, j])
+ for j in range(self.T_MPC-2):
+ self.ddq_next += (1./self.duration_MPC)**2 * self.BC(n-2, j) * t**j * (1-t)**(n-2-j) * n * (n-1)* (Q[:, j+2] - 2*Q[:, j+1] + Q[:, j])
+# ddp_right += (1./self.duration_MPC)**2 * self.BC(self.n-2, j) * t**j * (1-t)**(self.n-2-j) * self.n * (self.n-1)* (P_Right[:, j+2] - 2*P_Right[:, j+1] + P_Right[:, j])
+
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., self.dq_next[2]])
+ Global_v_b = np.asarray([self.dq_next[0], self.dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+
+# print(p_right)
+# print(self.Derivation_RARM_pos_start)
+
+ self.duration_MPC_planner = self.duration - self._idx/self._freq
+
+ self.eva_trajectory.header.stamp = rospy.Time.now()
+ self.eva_trajectory.points[0].positions = self.q_next[-self.ndof_position_control:].tolist()
+ # update message
+ self._msg.data = self.q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]; self._msg_velocity.linear.y = Local_v_b[1]; self._msg_velocity.angular.z = Local_w_b[2];
+ self._msg_acceleration.data = self.ddq_next[-self.ndof:]
+ # publish message
+ self._joint_pub.publish(self.eva_trajectory)
+# self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ self._joint_acc_pub.publish(self._msg_acceleration)
+
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+
+ print(rospy.get_time() - time_begin)
+
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ self.start_RARM_force = self._RARM_ee_force_trajectory(self.duration)
+ self.start_RARM_torque = self._RARM_ee_torque_trajectory(self.duration)
+ self.start_LARM_force = self._LARM_ee_force_trajectory(self.duration)
+ self.start_LARM_torque = self._LARM_ee_torque_trajectory(self.duration)
+
+ self.solution_MPC_planner = None
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ self.dq_curr_base = [float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y), float(msg.twist.twist.angular.z)]
+
+ def read_right_ee_grasp_ft_data_cb(self, msg):
+ self.F_ext_Right = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0])
+
+ def read_left_ee_grasp_ft_data_cb(self, msg):
+ self.F_ext_Left = np.asarray([ msg.data[0], msg.data[1], msg.data[2], 0, msg.data[4], 0 ])
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+ def BC(self, n, i):
+ return np.math.factorial(n)/(np.math.factorial(i) * (np.math.factorial(n-i)))
+
+ def skew(self, vec):
+ A = np.asarray([ [0, -vec[2], vec[1]], [vec[2], 0, -vec[0]], [-vec[1], vec[0], 0]])
+ return A
+
+ def angular_velocity_to_quaternionRate(self, quaternion):
+# A = 0.5* np.asarray([ [ quaternion[3], quaternion[2], -quaternion[1] ],
+# [ -quaternion[2], quaternion[3], quaternion[0] ],
+# [ quaternion[1], -quaternion[0], quaternion[3] ],
+# [ -quaternion[0], -quaternion[1], -quaternion[2] ]
+# ])
+ A = optas.casadi.SX(np.zeros((4, 3)))
+ A[0,0] = quaternion[3]; A[0,1] = quaternion[2]; A[0,2] = -quaternion[1];
+ A[1,0] = -quaternion[2]; A[1,1] = quaternion[3]; A[1,2] = quaternion[0];
+ A[2,0] = quaternion[1]; A[2,1] = -quaternion[0]; A[2,2] = quaternion[3];
+ A[3,0] = -quaternion[0]; A[3,1] = -quaternion[1]; A[3,2] = -quaternion[2];
+ return 0.5*A
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_MPC_BC_operational_AD", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_fixedbase.py b/script/action_server_cmd_pose_fixedbase.py
new file mode 100755
index 0000000..139a086
--- /dev/null
+++ b/script/action_server_cmd_pose_fixedbase.py
@@ -0,0 +1,393 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+
+from sensor_msgs.msg import JointState
+# ROS messages types for the real robot
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+# ROS messages for command configuration action
+# from pushing_msgs.msg import CmdPoseAction, CmdPoseFeedback, CmdPoseResult
+from pushing_msgs.msg import CmdDualPoseAction, CmdDualPoseFeedback, CmdDualPoseResult
+from geometry_msgs.msg import Twist
+
+from urdf_parser_py.urdf import URDF
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', 0.2)
+ self._x_max = rospy.get_param('~x_max', 0.8)
+ self._y_min = rospy.get_param('~y_min', -0.5)
+ self._y_max = rospy.get_param('~y_max', 0.5)
+ self._z_min = rospy.get_param('~z_min', 0.75)
+ self._z_max = rospy.get_param('~z_max', 1.5)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 100)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names_position_control = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed' and jnt.type !='continuous')]
+ self.ndof_position_control = len(self.joint_names_position_control)
+ ### optas
+ ### ---------------------------------------------------------
+ # set up right arm optimizataion
+ right_arm = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5'],
+ name='chonk_right_arm'
+ )
+ self.right_arm_name = right_arm.get_name()
+ self.ndof = right_arm.ndof
+ # nominal robot configuration
+ self.q_nom = optas.DM.zeros(self.ndof)
+ self.right_arm_opt_idx = right_arm.optimized_joint_indexes
+ self.right_arm_param_idx = right_arm.parameter_joint_indexes
+ # set up optimization builder
+ builder_right_arm = optas.OptimizationBuilder(T=1, robots=[right_arm])
+ # get robot state and parameters variables
+ q_var = builder_right_arm.get_robot_states_and_parameters(self.right_arm_name)
+ # get end-effector pose as parameters
+ pos = builder_right_arm.add_parameter('pos', 3)
+ ori = builder_right_arm.add_parameter('ori', 4)
+ # set variable boudaries
+ builder_right_arm.enforce_model_limits(self.right_arm_name)
+ # equality constraint on right arm position
+ pos_fnc = right_arm.get_global_link_position_function(link=self._link_ee_right)
+ builder_right_arm.add_equality_constraint('final_pos', pos_fnc(q_var), rhs=pos)
+ # rotation of the right arm position
+ self.R_fnc = right_arm.get_global_link_rotation_function(link=self._link_ee_right)
+ # equality constraint on orientation
+ ori_fnc = right_arm.get_global_link_quaternion_function(link=self._link_ee_right)
+ builder_right_arm.add_equality_constraint('final_ori', ori_fnc(q_var), rhs=ori)
+ # optimization cost: close to nominal config
+ builder_right_arm.add_cost_term('nom_config', optas.sumsqr(q_var-self.q_nom))
+ # setup solver
+ self.solver_right_arm = optas.CasADiSolver(optimization=builder_right_arm.build()).setup('ipopt')
+ ### ---------------------------------------------------------
+ # set up head optimization
+ head = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['CHEST_JOINT0', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_head'
+ )
+ self.head_name = head.get_name()
+ self.head_opt_idx = head.optimized_joint_indexes
+ self.head_param_idx = head.parameter_joint_indexes
+ # set up optimization builder
+ builder_head = optas.OptimizationBuilder(T=1, robots=[head])
+ # get robot state and parameters variables
+ q_var = builder_head.get_robot_states_and_parameters(self.head_name)
+ # get end-effector pose as parameters
+ pos = builder_head.add_parameter('pos', 3)
+ # get head heading
+ pos_head = head.get_global_link_position_function(link=self._link_head)
+ # get gaze position
+ self.pos_gaze_fnc = head.get_global_link_position_function(link=self._link_gaze)
+ # set variable boudaries
+ builder_head.enforce_model_limits(self.head_name)
+ # optimization cost: close to nominal config
+ builder_head.add_cost_term('heading', optas.norm_2(pos_head(q_var)-pos))
+ # setup solver
+ self.solver_head = optas.CasADiSolver(optimization=builder_head.build()).setup('ipopt')
+ ### ---------------------------------------------------------
+ # set up left arm optimizataion
+ left_arm = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_left_arm'
+ )
+ self.left_arm_name = left_arm.get_name()
+ # nominal robot configuration
+ self.left_arm_opt_idx = left_arm.optimized_joint_indexes
+ self.left_arm_param_idx = left_arm.parameter_joint_indexes
+ # set up optimization builder
+ builder_left_arm = optas.OptimizationBuilder(T=1, robots=[left_arm])
+ # get robot state and parameters variables
+ q_var = builder_left_arm.get_robot_states_and_parameters(self.left_arm_name)
+ q_opt = builder_left_arm.get_model_states(self.left_arm_name)
+ # get end-effector pose as parameters
+ pos = builder_left_arm.add_parameter('pos', 3)
+ # set variable boudaries
+ builder_left_arm.enforce_model_limits(self.left_arm_name)
+ # equality constraint on position
+ pos_fnc = left_arm.get_global_link_position_function(link=self._link_ee_left)
+ builder_left_arm.add_equality_constraint('final_pos', pos_fnc(q_var), rhs=pos)
+ # optimization cost: close to nominal config
+ builder_left_arm.add_cost_term('nom_config', optas.sumsqr(q_opt-self.q_nom[self.left_arm_opt_idx]))
+ # setup solver
+ self.solver_left_arm = optas.CasADiSolver(optimization=builder_left_arm.build()).setup('ipopt')
+ ### ---------------------------------------------------------
+ # initialize variables
+ self.q_curr = np.zeros(self.ndof)
+ self.joint_names = []
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher(
+ self._pub_cmd_topic_name,
+ Float64MultiArray,
+ queue_size=10
+ )
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher(
+ "/chonk/donkey_velocity_controller/cmd_vel",
+ Twist,
+ queue_size=10
+ )
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ # initialize action messages
+ self._feedback = CmdDualPoseFeedback()
+ self._result = CmdDualPoseResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'cmd_pose',
+ CmdDualPoseAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_R = np.asarray([
+ acceped_goal.poseR.position.x,
+ acceped_goal.poseR.position.y,
+ acceped_goal.poseR.position.z
+ ])
+ pos_L = np.asarray([
+ acceped_goal.poseL.position.x,
+ acceped_goal.poseL.position.y,
+ acceped_goal.poseL.position.z
+ ])
+ ori_T = np.asarray([
+ acceped_goal.poseR.orientation.x,
+ acceped_goal.poseR.orientation.y,
+ acceped_goal.poseR.orientation.z,
+ acceped_goal.poseR.orientation.w
+ ])
+ # check boundaries of the position
+ if (pos_R > self._pos_max).any() or (pos_R < self._pos_min).any():
+ rospy.logwarn("%s: Request aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_R[0], pos_R[1], pos_R[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ if (pos_L > self._pos_max).any() or (pos_L < self._pos_min).any():
+ rospy.logwarn("%s: Lequest aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_L[0], pos_L[1], pos_L[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to (%.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name,
+ pos_R[0], pos_R[1], pos_R[2],
+ ori_T[0], ori_T[1], ori_T[2], ori_T[3],
+ pos_L[0], pos_L[1], pos_L[2],
+ acceped_goal.duration
+ )
+ )
+ # read current robot joint positions
+ q0 = self.q_curr
+ qT = np.zeros(self.ndof)
+ ### optas
+ ### ---------------------------------------------------------
+ ### right hand problem
+ # set initial seed
+ self.solver_right_arm.reset_initial_seed({f'{self.right_arm_name}/q': self.q_nom[self.right_arm_opt_idx]})
+ self.solver_right_arm.reset_parameters({'pos': pos_R, 'ori': ori_T, f'{self.right_arm_name}/P': self.q_nom[self.right_arm_param_idx]})
+ # solve problem
+ solution = self.solver_right_arm.solve()
+ qT_array = solution[f'{self.right_arm_name}/q']
+ # save solution
+ qT[self.right_arm_opt_idx] = np.asarray(qT_array).T[0]
+ qT[self.right_arm_param_idx] = np.asarray(self.q_nom[self.right_arm_param_idx]).T[0]
+ ### ---------------------------------------------------------
+ ### head problem
+ # set initial seed
+ self.solver_head.reset_initial_seed({f'{self.head_name}/q': qT[self.head_opt_idx]})
+ self.solver_head.reset_parameters({'pos': self.pos_gaze_fnc(qT), f'{self.right_arm_name}/P': qT[self.head_param_idx]})
+ # solve problem
+ solution = self.solver_head.solve()
+ qT_array = solution[f'{self.head_name}/q']
+ # save solution
+ qT[self.head_opt_idx] = np.asarray(qT_array).T[0]
+ ### ---------------------------------------------------------
+ ### left arm problem
+ # set initial seed
+ self.solver_left_arm.reset_initial_seed({f'{self.left_arm_name}/q': qT[self.left_arm_opt_idx]})
+ self.solver_left_arm.reset_parameters({'pos': pos_L, f'{self.right_arm_name}/P': qT[self.left_arm_param_idx]})
+ # solve problem
+ solution = self.solver_left_arm.solve()
+ qT_array = solution[f'{self.left_arm_name}/q']
+ # save solution
+ qT[self.left_arm_opt_idx] = np.asarray(qT_array).T[0]
+ ### ---------------------------------------------------------
+ # helper variables
+ T = acceped_goal.duration
+ # print goal request
+ rospy.loginfo("%s: Request to send robot joints to %s in %.1f seconds." % (self._name, qT, T))
+ self._steps = int(T * self._freq)
+ self._idx = 0
+ Dq = qT - q0
+ # interpolate between current and target configuration
+ # polynomial obtained for zero speed (3rd order) and acceleratin (5th order)
+ # at the initial and final time
+ # self._q = lambda t: q0 + (3.*((t/T)**2) - 2.*((t/T)**3))*Dq # 3rd order
+ self._q = lambda t: q0 + (10.*((t/T)**3) - 15.*((t/T)**4) + 6.*((t/T)**5))*Dq # 5th order
+ self._t = np.linspace(0., T, self._steps + 1)
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+ # compute next configuration with lambda function
+ q_next = self._q(self._t[self._idx])
+ # update message
+# self._msg.data = q_next
+ self._msg.data = q_next[:self.ndof_position_control]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity) # This is for donkey_velocity_controller
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr = np.asarray(list(msg.position))
+ self.joint_names = msg.name
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_planner.py b/script/action_server_cmd_pose_planner.py
new file mode 100755
index 0000000..6cb0751
--- /dev/null
+++ b/script/action_server_cmd_pose_planner.py
@@ -0,0 +1,403 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseFeedback, CmdChonkPoseResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 100)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body motion optimization
+ wholebodyPlanner = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0, 1],
+ param_joints=[],
+ name='chonk_wholebodyPlanner'
+ )
+ self.wholebodyPlanner_name = wholebodyPlanner.get_name()
+ self.ndof = wholebodyPlanner.ndof
+ self.dt = 0.5 # time step
+ self.T = 11 # T is number of time steps
+ # nominal robot configuration
+ self.q_nom = optas.DM.zeros((self.ndof, self.T))
+ self.dq_nom = optas.DM.zeros((self.ndof, self.T))
+ self.wholebodyPlanner_opt_idx = wholebodyPlanner.optimized_joint_indexes
+ self.wholebodyPlanner_param_idx = wholebodyPlanner.parameter_joint_indexes
+ # set up optimization builder.
+ builder_wholebodyPlanner = optas.OptimizationBuilder(self.T, robots=[wholebodyPlanner], derivs_align=True)
+ # get robot state variables, get velocity state variables
+ q_var = builder_wholebodyPlanner.get_robot_states_and_parameters(self.wholebodyPlanner_name)
+ dq_var = builder_wholebodyPlanner.get_model_states(self.wholebodyPlanner_name, time_deriv=1)
+ # get end-effector pose as parameters
+ pos_R = builder_wholebodyPlanner.add_parameter('pos_R', 3)
+ ori_R = builder_wholebodyPlanner.add_parameter('ori_R', 4)
+ pos_L = builder_wholebodyPlanner.add_parameter('pos_L', 3)
+ ori_L = builder_wholebodyPlanner.add_parameter('ori_L', 4)
+ # set variable boudaries
+ builder_wholebodyPlanner.enforce_model_limits(self.wholebodyPlanner_name, time_deriv=0)
+ builder_wholebodyPlanner.enforce_model_limits(self.wholebodyPlanner_name, time_deriv=1)
+ # Constraint: dynamics
+ builder_wholebodyPlanner.integrate_model_states(self.wholebodyPlanner_name, time_deriv=1, dt=self.dt)
+ # Add parameters
+ init_position = builder_wholebodyPlanner.add_parameter('init_position', self.ndof) # initial robot position
+ init_velocity = builder_wholebodyPlanner.add_parameter('init_velocity', self.ndof) # initial robot velocity
+ manipulability = builder_wholebodyPlanner.add_parameter('m', 1) # manipulability measure
+ # manipulability functions of two arm end effectors
+ Right_manipulability_fnc = wholebodyPlanner.get_link_manipulability_function(link=self._link_ee_right, base_link = self._link_donkey)
+ Left_manipulability_fnc = wholebodyPlanner.get_link_manipulability_function(link=self._link_ee_left, base_link = self._link_donkey)
+ # constraint on manipulability of two arm end effectors
+ builder_wholebodyPlanner.add_geq_inequality_constraint('Right_manipulability_measure', Right_manipulability_fnc(q_var), rhs=25)
+ builder_wholebodyPlanner.add_geq_inequality_constraint('Left_manipulability_measure', Left_manipulability_fnc(q_var), rhs=25)
+ # Constraint: initial state
+ builder_wholebodyPlanner.fix_configuration(self.wholebodyPlanner_name, config=init_position)
+ builder_wholebodyPlanner.fix_configuration(self.wholebodyPlanner_name, config=init_velocity, time_deriv=1)
+ # Constraint: final velocity
+ dxF = builder_wholebodyPlanner.get_model_state(self.wholebodyPlanner_name, -1, time_deriv=1)
+ builder_wholebodyPlanner.add_equality_constraint('final_velocity', dxF)
+ # equality constraint on right and left arm positions
+ pos_fnc_Right = wholebodyPlanner.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = wholebodyPlanner.get_global_link_position_function(link=self._link_ee_left)
+ # rotation functions of the right and left arms
+ self.Rotation_fnc_Right = wholebodyPlanner.get_global_link_rotation_function(link=self._link_ee_right)
+ self.Rotation_fnc_Left = wholebodyPlanner.get_global_link_rotation_function(link=self._link_ee_left)
+ # quaternion functions of two arm end effectors
+ ori_fnc_Right = wholebodyPlanner.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = wholebodyPlanner.get_global_link_quaternion_function(link=self._link_ee_left)
+ # optimization cost: close to target
+ builder_wholebodyPlanner.add_cost_term('Right_arm position', optas.sumsqr(pos_fnc_Right(q_var)-pos_R))
+ builder_wholebodyPlanner.add_cost_term('Left_arm position', optas.sumsqr(pos_fnc_Left(q_var)-pos_L))
+ builder_wholebodyPlanner.add_cost_term('Right_arm orientation', optas.sumsqr(ori_fnc_Right(q_var)-ori_R))
+ builder_wholebodyPlanner.add_cost_term('Left_arm orientation', optas.sumsqr(ori_fnc_Left(q_var)-ori_L))
+
+
+ # Cost: minimize velocity
+# w = 0.01/float(self.T) # weight on cost term
+# builder_wholebodyPlanner.add_cost_term('minimize_velocity', w*optas.sumsqr(dq_var))
+
+ # Cost: minimize acceleration
+# w = 0.5/float(self.T) # weight on cost term
+# ddq_var = (dq_var[:, 1:] - dq_var[:, :-1])/self.dt
+# builder_wholebodyPlanner.add_cost_term('minimize_acceleration', w*optas.sumsqr(ddq_var))
+ # setup solver
+ self.solver_wholebodyPlanner = optas.CasADiSolver(optimization=builder_wholebodyPlanner.build()).setup('ipopt')
+ ### ---------------------------------------------------------
+ # initialize variables
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.dq_curr = np.zeros(self.ndof)
+ self.dq_curr_joint = np.zeros(self.ndof_position_control)
+ self.dq_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ self.qT = np.zeros((self.ndof, self.T))
+ self.dqT = np.zeros((self.ndof, self.T))
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ self.donkey_velocity = np.zeros(3)
+ self.donkey_angular_velocity = np.zeros(3)
+ self.duration = float(self.T-1)*self.dt
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/chonk/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+ self._joint_sub_base = rospy.Subscriber(
+ "/chonk/donkey_velocity_controller/odom",
+ Odometry,
+ self.read_base_states_cb
+ )
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher(
+ self._pub_cmd_topic_name,
+ Float64MultiArray,
+ queue_size=10
+ )
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher(
+ "/chonk/donkey_velocity_controller/cmd_vel",
+ Twist,
+ queue_size=10
+ )
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ # initialize action messages
+ self._feedback = CmdChonkPoseFeedback()
+ self._result = CmdChonkPoseResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'cmd_pose',
+ CmdChonkPoseAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([
+ acceped_goal.poseR.position.x,
+ acceped_goal.poseR.position.y,
+ acceped_goal.poseR.position.z
+ ])
+ pos_Left = np.asarray([
+ acceped_goal.poseL.position.x,
+ acceped_goal.poseL.position.y,
+ acceped_goal.poseL.position.z
+ ])
+ ori_Right = np.asarray([
+ acceped_goal.poseR.orientation.x,
+ acceped_goal.poseR.orientation.y,
+ acceped_goal.poseR.orientation.z,
+ acceped_goal.poseR.orientation.w
+ ])
+ ori_Left = np.asarray([
+ acceped_goal.poseL.orientation.x,
+ acceped_goal.poseL.orientation.y,
+ acceped_goal.poseL.orientation.z,
+ acceped_goal.poseL.orientation.w
+ ])
+ # check boundaries of the position
+ if (pos_Right > self._pos_max).any() or (pos_Right < self._pos_min).any():
+ rospy.logwarn("%s: Request aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Right[0], pos_Right[1], pos_Right[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ if (pos_Left > self._pos_max).any() or (pos_Left < self._pos_min).any():
+ rospy.logwarn("%s: Lequest aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Left[0], pos_Left[1], pos_Left[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name,
+ pos_Right[0], pos_Right[1], pos_Right[2],
+ ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2],
+ ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3],
+ acceped_goal.duration
+ )
+ )
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.dq_curr = np.concatenate((self.dq_curr_base, self.dq_curr_joint), axis=None)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+# self.q_curr = np.zeros(self.ndof)
+# self.dq_curr = np.zeros(self.ndof)
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve the whole-body planner problem
+ # set initial seed
+ self.solver_wholebodyPlanner.reset_initial_seed({f'{self.wholebodyPlanner_name}/q': self.q_nom, f'{self.wholebodyPlanner_name}/dq': self.dq_nom})
+ self.solver_wholebodyPlanner.reset_parameters({'init_position': self.q_curr, 'init_velocity': self.dq_curr, 'pos_R': pos_Right, 'ori_R': ori_Right, 'pos_L': pos_Left, 'ori_L': ori_Left})
+ # solve problem
+ solution = self.solver_wholebodyPlanner.solve()
+ qT_array = solution[f'{self.wholebodyPlanner_name}/q']
+ dqT_array = solution[f'{self.wholebodyPlanner_name}/dq']
+# print(qT_array)
+# print(dqT_array)
+ # save solution
+ self.qT = np.asarray(qT_array).T[self.T-1:]
+ self.dqT = np.asarray(dqT_array).T[self.T-1:]
+ self.plan_q = self.solver_wholebodyPlanner.interpolate(solution[f'{self.wholebodyPlanner_name}/q'], self.duration)
+ self.plan_dq = self.solver_wholebodyPlanner.interpolate(solution[f'{self.wholebodyPlanner_name}/dq'], self.duration)
+ ### ---------------------------------------------------------
+ # print goal request
+ rospy.loginfo("%s: Request to send robot joints to %s in %.1f seconds." % (self._name, self.qT, self.duration))
+ self._steps = int(self.duration * self._freq)
+ self._idx = 0
+ self._t = optas.np.linspace(0, self.duration, self._steps+1)
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+ # compute next configuration with lambda function
+ q_next = self.plan_q(self._t[self._idx])
+ dq_next = self.plan_dq(self._t[self._idx])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+ self.dq_curr_joint = np.asarray(list(msg.velocity)[:self.ndof_position_control])
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+ self.donkey_velocity = np.asarray([msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z])
+ self.donkey_angular_velocity = np.asarray([msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z])
+ base_linear_velocity_global = self.donkey_R @ self.donkey_velocity
+ base_angular_velocity_global = self.donkey_R @ self.donkey_angular_velocity
+ self.dq_curr_base = [base_linear_velocity_global[0], base_linear_velocity_global[1], base_angular_velocity_global[2]]
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_planner", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_cmd_pose_wholebody.py b/script/action_server_cmd_pose_wholebody.py
new file mode 100755
index 0000000..2733bde
--- /dev/null
+++ b/script/action_server_cmd_pose_wholebody.py
@@ -0,0 +1,386 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+#np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pushing_msgs.msg import CmdChonkPoseAction, CmdChonkPoseFeedback, CmdChonkPoseResult
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from urdf_parser_py.urdf import URDF
+import tf
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class CmdPoseActionServer(object):
+ """docstring for CmdPoseActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self._name = name
+ rospy.loginfo("%s: Initializing class", self._name)
+ ## get parameters:
+ ## --------------------------------------
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', -10000)
+ self._x_max = rospy.get_param('~x_max', 10000)
+ self._y_min = rospy.get_param('~y_min', -10000)
+ self._y_max = rospy.get_param('~y_max', 10000)
+ self._z_min = rospy.get_param('~z_min', -10000)
+ self._z_max = rospy.get_param('~z_max', 10000)
+ self._pos_min = np.asarray([self._x_min, self._y_min, self._z_min])
+ self._pos_max = np.asarray([self._x_max, self._y_max, self._z_max])
+ # robot name
+ # donkey_base frame
+ self._link_donkey = rospy.get_param('~link_donkey', 'link_donkey')
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 250)
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # load robot_description
+ param_robot_description = '~/robot_description_wholebody'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ self._urdf = URDF.from_parameter_server(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self._name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ self.joint_names = [jnt.name for jnt in self._urdf.joints if (jnt.type != 'fixed')]
+ self.ndof_base = 3
+ self.ndof_position_control = len(self.joint_names) - self.ndof_base
+ ### optas
+ ### ---------------------------------------------------------
+ # set up whole-body motion optimization
+ wholebody = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[0],
+ param_joints=[],
+ name='chonk_wholebody'
+ )
+ self.wholebody_name = wholebody.get_name()
+ self.ndof = wholebody.ndof
+ # nominal robot configuration
+ self.q_nom = optas.DM.zeros(self.ndof)
+ self.wholebody_opt_idx = wholebody.optimized_joint_indexes
+ self.wholebody_param_idx = wholebody.parameter_joint_indexes
+ # set up optimization builder
+ builder_wholebody = optas.OptimizationBuilder(T=1, robots=[wholebody])
+ # get robot state and parameters variables
+ q_var = builder_wholebody.get_robot_states_and_parameters(self.wholebody_name)
+ # get end-effector pose as parameters
+ pos_R = builder_wholebody.add_parameter('pos_R', 3)
+ ori_R = builder_wholebody.add_parameter('ori_R', 4)
+ pos_L = builder_wholebody.add_parameter('pos_L', 3)
+ ori_L = builder_wholebody.add_parameter('ori_L', 4)
+ # set variable boudaries
+ builder_wholebody.enforce_model_limits(self.wholebody_name)
+ # equality constraint on right and left arm positions
+ pos_fnc_Right = wholebody.get_global_link_position_function(link=self._link_ee_right)
+ pos_fnc_Left = wholebody.get_global_link_position_function(link=self._link_ee_left)
+ builder_wholebody.add_equality_constraint('final_pos_Right', pos_fnc_Right(q_var), rhs=pos_R)
+ builder_wholebody.add_equality_constraint('final_pos_Left', pos_fnc_Left(q_var), rhs=pos_L)
+ # rotation of the right and left arms
+ self.Rotation_fnc_Right = wholebody.get_global_link_rotation_function(link=self._link_ee_right)
+ self.Rotation_fnc_Left = wholebody.get_global_link_rotation_function(link=self._link_ee_left)
+ # equality constraint on orientations
+ ori_fnc_Right = wholebody.get_global_link_quaternion_function(link=self._link_ee_right)
+ ori_fnc_Left = wholebody.get_global_link_quaternion_function(link=self._link_ee_left)
+ builder_wholebody.add_equality_constraint('final_ori_Right', ori_fnc_Right(q_var), rhs=ori_R)
+ builder_wholebody.add_equality_constraint('final_ori_Left', ori_fnc_Left(q_var), rhs=ori_L)
+ # optimization cost: close to nominal config
+ builder_wholebody.add_cost_term('nom_config', optas.sumsqr(q_var-self.q_nom))
+ # setup solver
+ self.solver_wholebody = optas.CasADiSolver(optimization=builder_wholebody.build()).setup('knitro')
+
+# print(wholebody.get_global_geometric_jacobian(link=self._link_donkey, q=q_var))
+# print(wholebody.get_global_geometric_jacobian(link=self._link_donkey, q=q_var)[0,0])
+# print(wholebody.get_global_geometric_jacobian(link=self._link_donkey, q=q_var)[5,2])
+# print(wholebody.get_global_linear_jacobian(link=self._link_donkey, q=q_var)[0,0])
+# print(wholebody.get_global_angular_geometric_jacobian(link=self._link_donkey, q=q_var)[2,2])
+# print(wholebody.get_global_analytical_jacobian(link=self._link_donkey, q=q_var)[2,5])
+# print(wholebody.get_global_linear_jacobian(link=self._link_donkey, q=q_var)[0,0])
+# print(wholebody.get_global_angular_analytical_jacobian(link=self._link_donkey, q=q_var)[2,2])
+ ### ---------------------------------------------------------
+ # initialize variables
+ self.q_curr = np.zeros(self.ndof)
+ self.q_curr_joint = np.zeros(self.ndof_position_control)
+ self.q_curr_base = np.zeros(self.ndof_base)
+ self.joint_names_position = []
+ self.joint_names_base = ['base_joint_1', 'base_joint_2', 'base_joint_3']
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/chonk/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+ self._joint_sub_base = rospy.Subscriber(
+ "/chonk/donkey_velocity_controller/odom",
+ Odometry,
+ self.read_base_states_cb
+ )
+ # declare joint publisher
+# self._joint_pub = rospy.Publisher(
+# self._pub_cmd_topic_name,
+# Float64MultiArray,
+# queue_size=10
+# )
+ self._joint_pub = rospy.Publisher(
+ "/chonk/streaming_controller/command",
+ Float64MultiArray,
+ queue_size=10
+ )
+ # This is for donkey_velocity_controller
+ self._joint_pub_velocity = rospy.Publisher(
+ "/chonk/donkey_velocity_controller/cmd_vel",
+ Twist,
+ queue_size=10
+ )
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ # initialize action messages
+ self._feedback = CmdChonkPoseFeedback()
+ self._result = CmdChonkPoseResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'cmd_pose',
+ CmdChonkPoseAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ # desired end-effector position
+ pos_Right = np.asarray([
+ acceped_goal.poseR.position.x,
+ acceped_goal.poseR.position.y,
+ acceped_goal.poseR.position.z
+ ])
+ pos_Left = np.asarray([
+ acceped_goal.poseL.position.x,
+ acceped_goal.poseL.position.y,
+ acceped_goal.poseL.position.z
+ ])
+ ori_Right = np.asarray([
+ acceped_goal.poseR.orientation.x,
+ acceped_goal.poseR.orientation.y,
+ acceped_goal.poseR.orientation.z,
+ acceped_goal.poseR.orientation.w
+ ])
+ ori_Left = np.asarray([
+ acceped_goal.poseL.orientation.x,
+ acceped_goal.poseL.orientation.y,
+ acceped_goal.poseL.orientation.z,
+ acceped_goal.poseL.orientation.w
+ ])
+ # check boundaries of the position
+ if (pos_Right > self._pos_max).any() or (pos_Right < self._pos_min).any():
+ rospy.logwarn("%s: Request aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Right[0], pos_Right[1], pos_Right[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ if (pos_Left > self._pos_max).any() or (pos_Left < self._pos_min).any():
+ rospy.logwarn("%s: Lequest aborted. Goal position (%.2f, %.2f, %.2f) is outside of the workspace boundaries. Check parameters for this node." % (self._name, pos_Left[0], pos_Left[1], pos_Left[2]))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ # print goal request
+ rospy.loginfo("%s: Request to send right arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f), and left arm to position (%.2f, %.2f, %.2f) with orientation (%.2f, %.2f, %.2f, %.2f) in %.1f seconds." % (
+ self._name,
+ pos_Right[0], pos_Right[1], pos_Right[2],
+ ori_Right[0], ori_Right[1], ori_Right[2], ori_Right[3],
+ pos_Left[0], pos_Left[1], pos_Left[2],
+ ori_Left[0], ori_Left[1], ori_Left[2], ori_Left[3],
+ acceped_goal.duration
+ )
+ )
+ # read current robot joint positions
+ self.q_curr = np.concatenate((self.q_curr_base, self.q_curr_joint), axis=None)
+ self.joint_names = self.joint_names_base + self.joint_names_position
+ qT = np.zeros(self.ndof)
+ self.donkey_R = np.zeros((3, 3))
+ self.donkey_position = np.zeros(3)
+ ### optas
+ ### ---------------------------------------------------------
+ ### solve whole-body problem
+# self.q_nom[0] = acceped_goal.poseDonkey.position.x
+# self.q_nom[1] = acceped_goal.poseDonkey.position.y
+# self.q_nom[2] = tf.transformations.euler_from_quaternion([
+# acceped_goal.poseDonkey.orientation.x,
+# acceped_goal.poseDonkey.orientation.y,
+# acceped_goal.poseDonkey.orientation.z,
+# acceped_goal.poseDonkey.orientation.w ])[2]
+
+ # set initial seed
+ self.solver_wholebody.reset_initial_seed({f'{self.wholebody_name}/q': self.q_nom[self.wholebody_opt_idx]})
+ self.solver_wholebody.reset_parameters({'pos_R': pos_Right, 'ori_R': ori_Right, 'pos_L': pos_Left, 'ori_L': ori_Left, f'{self.wholebody_name}/P': self.q_nom[self.wholebody_param_idx]})
+# self.solver_wholebody.reset_parameters({'pos_R': pos_Right, 'ori_R': ori_Right, 'pos_L': pos_Left, f'{self.wholebody_name}/P': self.q_nom[self.wholebody_param_idx]})
+ print('1111111111111111111111111111111111111111111111111111111111111111111111')
+ # solve problem
+ solution = self.solver_wholebody.solve()
+ qT_array = solution[f'{self.wholebody_name}/q']
+ # save solution
+ qT[self.wholebody_opt_idx] = np.asarray(qT_array).T[0]
+ qT[self.wholebody_param_idx] = np.asarray(self.q_nom[self.wholebody_param_idx]).T[0]
+
+ ### ---------------------------------------------------------
+ # helper variables
+ q0 = self.q_curr
+ T = acceped_goal.duration
+ # print goal request
+ rospy.loginfo("%s: Request to send robot joints to %s in %.1f seconds." % (self._name, qT, T))
+ self._steps = int(T * self._freq)
+ self._idx = 0
+ Dq = qT - q0
+ # interpolate between current and target configuration polynomial obtained for zero speed (3rd order) and acceleratin (5th order)
+ # at the initial and final time
+ # self._q = lambda t: q0 + (3.*((t/T)**2) - 2.*((t/T)**3))*Dq # 3rd order
+ self._q = lambda t: q0 + (10.*((t/T)**3) - 15.*((t/T)**4) + 6.*((t/T)**5))*Dq # 5th order
+ self._dq = lambda t: (30.*((t/T)**2) - 60.*((t/T)**3) +30.*((t/T)**4))*(Dq/T)
+ self._t = np.linspace(0., T, self._steps + 1)
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof_position_control
+
+ self._msg_velocity = Twist()
+ self._msg_velocity.linear.x = 0
+ self._msg_velocity.linear.y = 0
+ self._msg_velocity.linear.z = 0
+ self._msg_velocity.angular.x = 0
+ self._msg_velocity.angular.y = 0
+ self._msg_velocity.angular.z = 0
+
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._idx < self._steps):
+ if(self._correct_mux_selection):
+ # increment idx (in here counts starts with 1)
+ self._idx += 1
+ # compute next configuration with lambda function
+ q_next = self._q(self._t[self._idx])
+ dq_next = self._dq(self._t[self._idx])
+ # compute the donkey velocity in its local frame
+ Global_w_b = np.asarray([0., 0., dq_next[2]])
+ Global_v_b = np.asarray([dq_next[0], dq_next[1], 0.])
+ Local_w_b = self.donkey_R.T @ Global_w_b
+ Local_v_b = self.donkey_R.T @ Global_v_b
+ # update message
+# self._msg.data[0:12] = q_next[-12:]
+# self._msg.data[12:15] = q_next[3:6]
+ self._msg.data = q_next[-self.ndof_position_control:]
+ self._msg_velocity.linear.x = Local_v_b[0]
+ self._msg_velocity.linear.y = Local_v_b[1]
+ self._msg_velocity.angular.z = Local_w_b[2]
+ # publish message
+ self._joint_pub.publish(self._msg)
+ self._joint_pub_velocity.publish(self._msg_velocity)
+ # compute progress
+ self._feedback.progress = (self._idx*100)/self._steps
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self._name))
+ self._result.reached_goal = False
+ self._action_server.set_aborted(self._result)
+ return
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ # set the action state to succeeded
+ rospy.loginfo("%s: Succeeded" % self._name)
+ self._result.reached_goal = True
+ self._action_server.set_succeeded(self._result)
+
+ return
+
+ def read_joint_states_cb(self, msg):
+ self.q_curr_joint = np.asarray(list(msg.position)[:self.ndof_position_control])
+ self.joint_names_position = msg.name[:self.ndof_position_control]
+
+ def read_base_states_cb(self, msg):
+ base_euler_angle = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
+ self.q_curr_base = [msg.pose.pose.position.x, msg.pose.pose.position.y, base_euler_angle[2]]
+ self.donkey_R = optas.spatialmath.rotz(base_euler_angle[2])
+ self.donkey_position = np.asarray([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ rospy.loginfo("%s: Preempted.", self._name)
+ # set the action state to preempted
+ self._action_server.set_preempted()
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("cmd_pose_server_wholebody", anonymous=True)
+ # Initialize node class
+ cmd_pose_server = CmdPoseActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/script/action_server_teleop_2d.py b/script/action_server_teleop_2d.py
new file mode 100755
index 0000000..04b1179
--- /dev/null
+++ b/script/action_server_teleop_2d.py
@@ -0,0 +1,390 @@
+#! /usr/bin/env python3
+
+# Copyright (C) 2022 Statistical Machine Learning and Motor Control Group (SLMC)
+# Authors: Joao Moura (maintainer)
+# email: joao.moura@ed.ac.uk
+
+# This file is part of iiwa_pushing package.
+
+# iiwa_pushing is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# iiwa_pushing is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+import sys
+import numpy as np
+np.set_printoptions(suppress=True)
+
+import rospy
+import actionlib
+import optas
+
+from scipy.spatial.transform import Rotation as R
+
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import JointState
+# ROS messages types for the real robot
+from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+# ROS messages for command configuration action
+from pushing_msgs.msg import TriggerCmdAction, TriggerCmdFeedback, TriggerCmdResult
+
+# For mux controller name
+from std_msgs.msg import String
+# service for selecting the controller
+from topic_tools.srv import MuxSelect
+
+class TeleOp2DActionServer(object):
+ """docstring for TeleOp2DActionServer."""
+
+ def __init__(self, name):
+ # initialization message
+ self.name = name
+ rospy.loginfo("%s: Initializing class", self.name)
+ #################################################################################
+ ## get parameters:
+ # workspace limit boundaries
+ self._x_min = rospy.get_param('~x_min', 0.2)
+ self._x_max = rospy.get_param('~x_max', 0.8)
+ self._y_min = rospy.get_param('~y_min', -0.5)
+ self._y_max = rospy.get_param('~y_max', 0.5)
+ # end-effector orientation limits
+ self._roll_min = np.rad2deg(rospy.get_param('~roll_min', 0.0))
+ self._roll_max = np.rad2deg(rospy.get_param('~roll_max', 0.0))
+ self._pitch_min = np.rad2deg(rospy.get_param('~pitch_min', 0.0))
+ self._pitch_max = np.rad2deg(rospy.get_param('~pitch_max', 0.0))
+ self._yaw_min = np.rad2deg(rospy.get_param('~yaw_min', -180.0))
+ self._yaw_max = np.rad2deg(rospy.get_param('~yaw_max', 180.0))
+ # safety gains on joint position and velocity limits
+ self._K_safety_lim_q = rospy.get_param('~K_safety_lim_q', 0.99)
+ self._K_safety_lim_q_dot = rospy.get_param('~K_safety_lim_q', 0.5)
+ # gains on the task cost optimization
+ self._K_pos = rospy.get_param('~K_pos', 100.)
+ self._K_ori = rospy.get_param('~K_ori', 1.)
+ # end-effector frame
+ self._link_ee_right = rospy.get_param('~link_ee_right', 'link_ee_right')
+ self._link_ee_left = rospy.get_param('~link_ee_left', 'link_ee_left')
+ self._link_head = rospy.get_param('~link_head', 'link_head')
+ self._link_ref = rospy.get_param('~link_ref', 'link_ref')
+ self._link_gaze = rospy.get_param('~link_gaze', 'link_gaze')
+ # control frequency
+ self._freq = rospy.get_param('~freq', 100)
+ self.dt = 1./self._freq
+ # publishing command node name
+ self._pub_cmd_topic_name = rospy.get_param('~cmd_topic_name', '/command')
+ # teleop commands in 2d isometric
+ self.joy_max = rospy.get_param('~joy_max', 0.68359375)
+ # velocity scalling from the normalized value
+ self.K_v_xy = rospy.get_param('~K_v_xy', 0.2)
+ self.K_w_z = rospy.get_param('~K_w_z', 0.5)
+ #################################################################################
+ # load robot_description
+ param_robot_description = '~/robot_description'
+ if rospy.has_param(param_robot_description):
+ self._robot_description = rospy.get_param(param_robot_description)
+ else:
+ rospy.logerr("%s: Param %s is unavailable!" % (self.name, param_robot_description))
+ rospy.signal_shutdown('Incorrect parameter name.')
+ #################################################################################
+ ### optas
+ # set up robot
+ right_arm = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[1],
+ param_joints=['HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5'],
+ name='chonk_right_arm'
+ )
+ self.right_arm_name = right_arm.get_name()
+ self.ndof = right_arm.ndof
+ # get right_arm limits
+ self.q_min_right_arm = self._K_safety_lim_q * right_arm.dlim[0][0]
+ self.q_max_right_arm = self._K_safety_lim_q * right_arm.dlim[0][1]
+ self.dq_min_right_arm = self.dt * self._K_safety_lim_q_dot * right_arm.dlim[1][0]
+ self.dq_max_right_arm = self.dt * self._K_safety_lim_q_dot * right_arm.dlim[1][1]
+ # nominal right_arm configuration
+ self.dq_nom = optas.DM.zeros(self.ndof)
+ self.right_arm_opt_idx = right_arm.optimized_joint_indexes
+ self.n_right_arm_opt = len(self.right_arm_opt_idx)
+ self.right_arm_param_idx = right_arm.parameter_joint_indexes
+ # set up optimization builder
+ builder_right_arm = optas.OptimizationBuilder(T=1, robots=[right_arm], derivs_align=True)
+ # get right_arm joint variables
+ dq_var = builder_right_arm.get_robot_states_and_parameters(self.right_arm_name, time_deriv=1)
+ dq_opt = builder_right_arm.get_model_states(self.right_arm_name, time_deriv=1)
+ # set parameters
+ q_var = builder_right_arm.add_parameter('q', self.ndof)
+ q_opt = q_var[self.right_arm_opt_idx]
+ dx = builder_right_arm.add_decision_variables('dx', 3)
+ dx_target = builder_right_arm.add_parameter('dx_target', 3)
+ z_target = builder_right_arm.add_parameter('z_target', 1)
+ # forward differential kinematics
+ self.fk = right_arm.get_global_link_position_function(link=self._link_ee_right)
+ self.J_pos = right_arm.get_global_link_linear_jacobian_function(link=self._link_ee_right)
+ self.quat = right_arm.get_link_quaternion_function(link=self._link_ee_right, base_link=self._link_ref)
+ # self.R = right_arm.get_global_link_rotation_function(link=self._link_ee_right)
+ self.R = right_arm.get_link_rotation_function(link=self._link_ee_right, base_link=self._link_ref)
+ self.J = right_arm.get_global_link_geometric_jacobian_function(link=self._link_ee_right)
+ self.rpy = right_arm.get_link_rpy_function(link=self._link_ee_right, base_link=self._link_ref)
+ self.J_rpy = right_arm.get_link_angular_analytical_jacobian_function(link=self._link_ee_right, base_link=self._link_ref)
+ # cost term
+ builder_right_arm.add_cost_term('cost_q', 1.0*optas.sumsqr(dq_opt))
+ builder_right_arm.add_cost_term('cost_pos', self._K_pos*optas.sumsqr(dx_target[0:2]-dx[0:2]))
+ builder_right_arm.add_cost_term('cost_ori', self._K_ori*optas.sumsqr(dx_target[2]-dx[2]))
+ # forward differential kinematics
+ builder_right_arm.add_equality_constraint('FDK_pos', (self.J(q_var)[0:2,:])@dq_var, dx[0:2])
+ builder_right_arm.add_equality_constraint('FDK_ori', (self.J_rpy(q_var)[2,:])@dq_var, dx[2])
+ # add joint position limits
+ builder_right_arm.add_bound_inequality_constraint('joint_pos_lim', self.q_min_right_arm, q_opt+dq_opt, self.q_max_right_arm)
+ # add joint velocity limitis
+ builder_right_arm.add_bound_inequality_constraint('joint_vel_lim', self.dq_min_right_arm, dq_opt, self.dq_max_right_arm)
+ # add height constraint
+ builder_right_arm.add_equality_constraint('height', self.fk(q_var)[2] + (self.J(q_var)[2,:])@dq_var, z_target)
+ # add end-effector yaw-pitch-yaw limits
+ builder_right_arm.add_bound_inequality_constraint('roll', self._roll_min, self.rpy(q_var)[0] + self.J_rpy(q_var)[0,:]@dq_var, self._roll_max)
+ builder_right_arm.add_bound_inequality_constraint('pitch', self._pitch_min, self.rpy(q_var)[1] + self.J_rpy(q_var)[1,:]@dq_var, self._pitch_max)
+ builder_right_arm.add_bound_inequality_constraint('yaw', self._yaw_min, self.rpy(q_var)[2] + self.J_rpy(q_var)[2,:]@dq_var, self._yaw_max)
+ # add workspace limits
+ builder_right_arm.add_bound_inequality_constraint('x_lim', self._x_min, self.fk(q_var)[0] + self.J(q_var)[0,:]@dq_var, self._x_max)
+ builder_right_arm.add_bound_inequality_constraint('y_lim', self._y_min, self.fk(q_var)[1] + self.J(q_var)[1,:]@dq_var, self._y_max)
+ # setup solver
+ self.solver_right_arm = optas.CasADiSolver(builder_right_arm.build()).setup(
+ solver_name='qpoases',
+ solver_options={'error_on_fail': False}
+ )
+ ### ---------------------------------------------------------
+ # set up head optimization
+ head = optas.RobotModel(
+ urdf_string=self._robot_description,
+ time_derivs=[1],
+ param_joints=['CHEST_JOINT0', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'],
+ name='chonk_head'
+ )
+ self.head_name = head.get_name()
+ self.head_opt_idx = head.optimized_joint_indexes
+ self.n_head_opt = len(self.head_opt_idx)
+ self.head_param_idx = head.parameter_joint_indexes
+ # get head limits
+ self.q_min_head = self._K_safety_lim_q * head.dlim[0][0]
+ self.q_max_head = self._K_safety_lim_q * head.dlim[0][1]
+ self.dq_min_head = self.dt * 0.5 * self._K_safety_lim_q_dot * head.dlim[1][0]
+ self.dq_max_head = self.dt * 0.5 * self._K_safety_lim_q_dot * head.dlim[1][1]
+ # get head heading
+ self.pos_head_fnc = head.get_global_link_position_function(link=self._link_head)
+ # get head gaze position
+ self.pos_gaze_fnc = head.get_global_link_position_function(link=self._link_gaze)
+ J_gaze = head.get_global_link_linear_jacobian_function(link=self._link_gaze)
+ # set up optimization builder
+ builder_head = optas.OptimizationBuilder(T=1, robots=[head], derivs_align=True)
+ # get head joint variables
+ dq_var = builder_head.get_robot_states_and_parameters(self.head_name, time_deriv=1)
+ dq_opt = builder_head.get_model_states(self.head_name, time_deriv=1)
+ # get end-effector pose as parameters
+ pos_head = builder_head.add_parameter('pos_head', 3)
+ pos_gaze = builder_head.add_parameter('pos_gaze', 3)
+ # set parameters
+ q_var = builder_head.add_parameter('q', self.ndof)
+ q_opt = q_var[self.head_opt_idx]
+ # add joint position limits
+ builder_head.add_bound_inequality_constraint('joint_pos_lim', self.q_min_head, q_opt+dq_opt, self.q_max_head)
+ # add joint velocity limitis
+ builder_head.add_bound_inequality_constraint('joint_vel_lim', self.dq_min_head, dq_opt, self.dq_max_head)
+ # optimization cost: close to nominal config
+ # builder
+ builder_head.add_cost_term('cost_dq', optas.norm_2(pos_head-pos_gaze+J_gaze(q_var)@dq_var))
+ # setup solver
+ self.solver_head = optas.CasADiSolver(builder_head.build()).setup(
+ solver_name='qpoases',
+ solver_options={'error_on_fail': False}
+ )
+ #################################################################################
+ # initialize variables
+ self.q_read = np.zeros(self.ndof)
+ self.q_cmd = np.zeros(self.ndof)
+ #################################################################################
+ # declare joint subscriber
+ self._joint_sub = rospy.Subscriber(
+ "/joint_states",
+ JointState,
+ self.read_joint_states_cb
+ )
+ # declare joystick subscriver
+ self._joy_sub = rospy.Subscriber(
+ "/spacenav/twist",
+ Twist,
+ self.read_twist_cb
+ )
+ # declare joint publisher
+ self._joint_pub = rospy.Publisher(
+ self._pub_cmd_topic_name,
+ Float64MultiArray,
+ queue_size=10
+ )
+ #################################################################################
+ # set mux controller selection as wrong by default
+ self._correct_mux_selection = False
+ # declare mux service
+ self._srv_mux_sel = rospy.ServiceProxy(rospy.get_namespace() + '/mux_joint_position/select', MuxSelect)
+ # declare subscriber for selected controller
+ self._sub_selected_controller = rospy.Subscriber(
+ "/mux_selected",
+ String,
+ self.read_mux_selection
+ )
+ #################################################################################
+ # initialize action messages
+ self._feedback = TriggerCmdFeedback()
+ self._result = TriggerCmdResult()
+ # declare action server
+ self._action_server = actionlib.SimpleActionServer(
+ 'teleop_2d',
+ TriggerCmdAction,
+ execute_cb=None,
+ auto_start=False
+ )
+ # register the preempt callback
+ self._action_server.register_goal_callback(self.goal_cb)
+ self._action_server.register_preempt_callback(self.preempt_cb)
+ # start action server
+ self._action_server.start()
+ #################################################################################
+
+ def goal_cb(self):
+ # activate publishing command
+ self._srv_mux_sel(self._pub_cmd_topic_name)
+ # accept the new goal request
+ acceped_goal = self._action_server.accept_new_goal()
+ self.target_pos_z = acceped_goal.pos_z
+ # read current robot joint positions for memory
+ self.q_cmd = self.q_read
+ ### optas
+ ### ---------------------------------------------------------
+ # initialize the message
+ self._msg = Float64MultiArray()
+ self._msg.layout = MultiArrayLayout()
+ self._msg.layout.data_offset = 0
+ self._msg.layout.dim.append(MultiArrayDimension())
+ self._msg.layout.dim[0].label = "columns"
+ self._msg.layout.dim[0].size = self.ndof
+ # create timer
+ dur = rospy.Duration(1.0/self._freq)
+ self._timer = rospy.Timer(dur, self.timer_cb)
+
+ def timer_cb(self, event):
+ """ Publish the robot configuration """
+
+ # make sure that the action is active
+ if(not self._action_server.is_active()):
+ self._timer.shutdown()
+ rospy.logwarn("%s: The action server is NOT active!")
+ self._result.trigger_off = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ # main execution
+ if(self._correct_mux_selection):
+ # current config
+ q_curr = self.q_cmd
+ # target displacement
+ dx_target = [
+ self.dt * self.v_x,
+ self.dt * self.v_y,
+ self.dt * self.w_z
+ ]
+ dq = np.zeros(self.ndof)
+ ### ---------------------------------------------------------
+ ### right hand problem
+ self.solver_right_arm.reset_initial_seed({f'{self.right_arm_name}/dq': self.dq_nom[self.right_arm_opt_idx]})
+ self.solver_right_arm.reset_parameters({
+ f'{self.right_arm_name}/dP': self.dq_nom[self.right_arm_param_idx],
+ 'q': q_curr,
+ 'dx_target': dx_target,
+ 'z_target': self.target_pos_z
+ })
+ # solve problem
+ solution = self.solver_right_arm.solve()
+ if self.solver_right_arm.did_solve():
+ dq_array = np.asarray(solution[f'{self.right_arm_name}/dq']).T[0]
+ else:
+ rospy.logwarn("%s: Right arm QP fail to find a solution!" % self.name)
+ dq_array = np.zeros(self.n_right_arm_opt)
+ dq[self.right_arm_opt_idx] = dq_array
+ ### ---------------------------------------------------------
+ ### head problem
+ self.solver_head.reset_initial_seed({f'{self.head_name}/dq': self.dq_nom[self.head_opt_idx]})
+ self.solver_head.reset_parameters({
+ f'{self.head_name}/dP': self.dq_nom[self.head_param_idx],
+ 'pos_head': self.pos_head_fnc(q_curr),
+ 'pos_gaze': self.pos_gaze_fnc(q_curr)
+ })
+ # solve problem
+ solution = self.solver_head.solve()
+ if self.solver_head.did_solve():
+ dq_array = np.asarray(solution[f'{self.head_name}/dq']).T[0]
+ else:
+ rospy.logwarn("%s: Head QP fail to find a solution!" % self.name)
+ dq_array = np.zeros(self.n_head_opt)
+ dq[self.head_opt_idx] = dq_array
+ ### ---------------------------------------------------------
+ # integrate solution
+ self.q_cmd = q_curr + dq
+ # update message
+ self._msg.data = self.q_cmd
+ # publish message
+ self._joint_pub.publish(self._msg)
+ # compute progress
+ self._feedback.is_active = True
+ # publish feedback
+ self._action_server.publish_feedback(self._feedback)
+ else:
+ # shutdown this timer
+ self._timer.shutdown()
+ rospy.logwarn("%s: Request aborted. The controller selection changed!" % (self.name))
+ self._result.trigger_off = False
+ self._action_server.set_aborted(self._result)
+ return
+
+ def read_twist_cb(self, msg):
+ # read planar part of the twist
+ v_x = msg.linear.x
+ v_y = msg.linear.y
+ w_z = msg.angular.z
+ # make v_x and v_y isometric
+ v_ang = np.arctan2(v_y, v_x)
+ c_ang = np.cos(v_ang)
+ s_ang = np.sin(v_ang)
+ if v_ang > np.deg2rad(-45.) and v_ang < np.deg2rad(45.):
+ norm_max = self.joy_max/c_ang
+ elif v_ang > np.deg2rad(45.) and v_ang < np.deg2rad(135.):
+ norm_max = self.joy_max/s_ang
+ elif v_ang < np.deg2rad(-45.) and v_ang > np.deg2rad(-135.):
+ norm_max = -self.joy_max/s_ang
+ else:
+ norm_max = -self.joy_max/c_ang
+ # save values
+ self.v_x = self.K_v_xy * (v_x / norm_max)
+ self.v_y = self.K_v_xy * (v_y / norm_max)
+ self.w_z = - self.K_w_z * (w_z / self.joy_max)
+
+ def read_joint_states_cb(self, msg):
+ self.q_read = np.asarray(list(msg.position))
+
+ def read_mux_selection(self, msg):
+ self._correct_mux_selection = (msg.data == self._pub_cmd_topic_name)
+
+ def preempt_cb(self):
+ self._timer.shutdown()
+ rospy.loginfo("%s: Client preempted this action.", self.name)
+ self._result.trigger_off = True
+ # set the action state to preempted
+ self._action_server.set_preempted(self._result)
+
+if __name__=="__main__":
+ # Initialize node
+ rospy.init_node("teleop_2d_server", anonymous=True)
+ # Initialize node class
+ teleop_2d_server = TeleOp2DActionServer(rospy.get_name())
+ # executing node
+ rospy.spin()
diff --git a/urdf/chonk.gv b/urdf/chonk.gv
new file mode 100644
index 0000000..982a6da
--- /dev/null
+++ b/urdf/chonk.gv
@@ -0,0 +1,185 @@
+digraph G {
+node [shape=box];
+"base_link" [label="base_link"];
+"donkey_base" [label="donkey_base"];
+"fl_wheel_link" [label="fl_wheel_link"];
+"fl_wheel_link_passive" [label="fl_wheel_link_passive"];
+"fr_wheel_link" [label="fr_wheel_link"];
+"fr_wheel_link_passive" [label="fr_wheel_link_passive"];
+"rl_wheel_link" [label="rl_wheel_link"];
+"rl_wheel_link_passive" [label="rl_wheel_link_passive"];
+"rr_wheel_link" [label="rr_wheel_link"];
+"rr_wheel_link_passive" [label="rr_wheel_link_passive"];
+"WAIST" [label="WAIST"];
+"CHEST_JOINT0_Link" [label="CHEST_JOINT0_Link"];
+"HEAD_JOINT0_Link" [label="HEAD_JOINT0_Link"];
+"HEAD_JOINT1_Link" [label="HEAD_JOINT1_Link"];
+"CAMERA_HEAD_HEADING_Link" [label="CAMERA_HEAD_HEADING_Link"];
+"CAMERA_HEAD_L_Link" [label="CAMERA_HEAD_L_Link"];
+"CAMERA_HEAD_R_Link" [label="CAMERA_HEAD_R_Link"];
+"CAMERA_HEAD_Link" [label="CAMERA_HEAD_Link"];
+"intel_realsense_d435_Mount_Link" [label="intel_realsense_d435_Mount_Link"];
+"intel_realsense_bottom_screw_frame" [label="intel_realsense_bottom_screw_frame"];
+"intel_realsense_link" [label="intel_realsense_link"];
+"intel_realsense_color_frame" [label="intel_realsense_color_frame"];
+"intel_realsense_color_optical_frame" [label="intel_realsense_color_optical_frame"];
+"intel_realsense_depth_frame" [label="intel_realsense_depth_frame"];
+"intel_realsense_depth_optical_frame" [label="intel_realsense_depth_optical_frame"];
+"intel_realsense_infra1_frame" [label="intel_realsense_infra1_frame"];
+"intel_realsense_infra1_optical_frame" [label="intel_realsense_infra1_optical_frame"];
+"intel_realsense_infra2_frame" [label="intel_realsense_infra2_frame"];
+"intel_realsense_infra2_optical_frame" [label="intel_realsense_infra2_optical_frame"];
+"LARM_JOINT0_Link" [label="LARM_JOINT0_Link"];
+"LARM_JOINT1_Link" [label="LARM_JOINT1_Link"];
+"LARM_JOINT2_Link" [label="LARM_JOINT2_Link"];
+"LARM_JOINT3_Link" [label="LARM_JOINT3_Link"];
+"LARM_JOINT4_Link" [label="LARM_JOINT4_Link"];
+"LARM_JOINT5_Link" [label="LARM_JOINT5_Link"];
+"LARM_ft_link_base" [label="LARM_ft_link_base"];
+"LARM_ft_Sensor_Link" [label="LARM_ft_Sensor_Link"];
+"LARM_ft_link" [label="LARM_ft_link"];
+"LARM_changer_link_base" [label="LARM_changer_link_base"];
+"LARM_changer_link" [label="LARM_changer_link"];
+"LARM_END_EFFECTOR" [label="LARM_END_EFFECTOR"];
+"LARM_END_EFFECTOR_finger" [label="LARM_END_EFFECTOR_finger"];
+"LARM_END_EFFECTOR_grasp" [label="LARM_END_EFFECTOR_grasp"];
+"LARM_END_EFFECTOR_grasp_end" [label="LARM_END_EFFECTOR_grasp_end"];
+"RARM_JOINT0_Link" [label="RARM_JOINT0_Link"];
+"RARM_JOINT1_Link" [label="RARM_JOINT1_Link"];
+"RARM_JOINT2_Link" [label="RARM_JOINT2_Link"];
+"RARM_JOINT3_Link" [label="RARM_JOINT3_Link"];
+"RARM_JOINT4_Link" [label="RARM_JOINT4_Link"];
+"RARM_JOINT5_Link" [label="RARM_JOINT5_Link"];
+"RARM_ft_link_base" [label="RARM_ft_link_base"];
+"RARM_ft_Sensor_Link" [label="RARM_ft_Sensor_Link"];
+"RARM_ft_link" [label="RARM_ft_link"];
+"RARM_changer_link_base" [label="RARM_changer_link_base"];
+"RARM_changer_link" [label="RARM_changer_link"];
+"RARM_END_EFFECTOR" [label="RARM_END_EFFECTOR"];
+"RARM_END_EFFECTOR_finger" [label="RARM_END_EFFECTOR_finger"];
+"gaze_ref" [label="gaze_ref"];
+"RARM_END_EFFECTOR_grasp" [label="RARM_END_EFFECTOR_grasp"];
+"RARM_END_EFFECTOR_grasp_end" [label="RARM_END_EFFECTOR_grasp_end"];
+"teleop_ref" [label="teleop_ref"];
+node [shape=ellipse, color=blue, fontcolor=blue];
+"base_link" -> "donkey_base_link_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"donkey_base_link_joint" -> "donkey_base"
+"donkey_base" -> "fl_wheel_joint" [label="xyz: 0.36 0.25 0.095 \nrpy: 0 -0 0"]
+"fl_wheel_joint" -> "fl_wheel_link"
+"fl_wheel_link" -> "fl_wheel_link_passive_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"fl_wheel_link_passive_joint" -> "fl_wheel_link_passive"
+"donkey_base" -> "fr_wheel_joint" [label="xyz: 0.36 -0.25 0.095 \nrpy: 0 -0 0"]
+"fr_wheel_joint" -> "fr_wheel_link"
+"fr_wheel_link" -> "fr_wheel_link_passive_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"fr_wheel_link_passive_joint" -> "fr_wheel_link_passive"
+"donkey_base" -> "rl_wheel_joint" [label="xyz: -0.36 0.25 0.095 \nrpy: 0 -0 0"]
+"rl_wheel_joint" -> "rl_wheel_link"
+"rl_wheel_link" -> "rl_wheel_link_passive_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"rl_wheel_link_passive_joint" -> "rl_wheel_link_passive"
+"donkey_base" -> "rr_wheel_joint" [label="xyz: -0.36 -0.25 0.095 \nrpy: 0 -0 0"]
+"rr_wheel_joint" -> "rr_wheel_link"
+"rr_wheel_link" -> "rr_wheel_link_passive_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"rr_wheel_link_passive_joint" -> "rr_wheel_link_passive"
+"base_link" -> "donkey_base_mount_joint" [label="xyz: 0.422 0 0.855 \nrpy: 0 -0 0"]
+"donkey_base_mount_joint" -> "WAIST"
+"WAIST" -> "CHEST_JOINT0" [label="xyz: 0 0 0.267 \nrpy: 0 -0 0"]
+"CHEST_JOINT0" -> "CHEST_JOINT0_Link"
+"CHEST_JOINT0_Link" -> "HEAD_JOINT0" [label="xyz: 0 0 0.302 \nrpy: 0 -0 0"]
+"HEAD_JOINT0" -> "HEAD_JOINT0_Link"
+"HEAD_JOINT0_Link" -> "HEAD_JOINT1" [label="xyz: 0 0 0.08 \nrpy: 0 -0 0"]
+"HEAD_JOINT1" -> "HEAD_JOINT1_Link"
+"HEAD_JOINT1_Link" -> "CAMERA_HEAD_HEADING_Joint" [label="xyz: 1.2 0 0 \nrpy: 0 -0 0"]
+"CAMERA_HEAD_HEADING_Joint" -> "CAMERA_HEAD_HEADING_Link"
+"HEAD_JOINT1_Link" -> "CAMERA_HEAD_L" [label="xyz: 0.0175 0.0725 0.105 \nrpy: 0 0.25 0"]
+"CAMERA_HEAD_L" -> "CAMERA_HEAD_L_Link"
+"HEAD_JOINT1_Link" -> "CAMERA_HEAD_R" [label="xyz: 0.0175 -0.0725 0.105 \nrpy: 0 0.25 0"]
+"CAMERA_HEAD_R" -> "CAMERA_HEAD_R_Link"
+"CAMERA_HEAD_R_Link" -> "CAMERA_HEAD_Joint" [label="xyz: 0 0.0725 0 \nrpy: 0 -0 0"]
+"CAMERA_HEAD_Joint" -> "CAMERA_HEAD_Link"
+"HEAD_JOINT1_Link" -> "intel_realsense_d435_mount_joint" [label="xyz: 0 0 0.144832 \nrpy: 0 0.249021 0"]
+"intel_realsense_d435_mount_joint" -> "intel_realsense_d435_Mount_Link"
+"intel_realsense_d435_Mount_Link" -> "intel_realsense_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"intel_realsense_joint" -> "intel_realsense_bottom_screw_frame"
+"intel_realsense_bottom_screw_frame" -> "intel_realsense_link_joint" [label="xyz: 0.0106 0.0175 0.0125 \nrpy: 0 -0 0"]
+"intel_realsense_link_joint" -> "intel_realsense_link"
+"intel_realsense_link" -> "intel_realsense_color_joint" [label="xyz: 0 0.015 0 \nrpy: 0 -0 0"]
+"intel_realsense_color_joint" -> "intel_realsense_color_frame"
+"intel_realsense_color_frame" -> "intel_realsense_color_optical_joint" [label="xyz: 0 0 0 \nrpy: -1.5708 -0 -1.5708"]
+"intel_realsense_color_optical_joint" -> "intel_realsense_color_optical_frame"
+"intel_realsense_link" -> "intel_realsense_depth_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"intel_realsense_depth_joint" -> "intel_realsense_depth_frame"
+"intel_realsense_depth_frame" -> "intel_realsense_depth_optical_joint" [label="xyz: 0 0 0 \nrpy: -1.5708 -0 -1.5708"]
+"intel_realsense_depth_optical_joint" -> "intel_realsense_depth_optical_frame"
+"intel_realsense_link" -> "intel_realsense_infra1_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"intel_realsense_infra1_joint" -> "intel_realsense_infra1_frame"
+"intel_realsense_infra1_frame" -> "intel_realsense_infra1_optical_joint" [label="xyz: 0 0 0 \nrpy: -1.5708 -0 -1.5708"]
+"intel_realsense_infra1_optical_joint" -> "intel_realsense_infra1_optical_frame"
+"intel_realsense_link" -> "intel_realsense_infra2_joint" [label="xyz: 0 -0.05 0 \nrpy: 0 -0 0"]
+"intel_realsense_infra2_joint" -> "intel_realsense_infra2_frame"
+"intel_realsense_infra2_frame" -> "intel_realsense_infra2_optical_joint" [label="xyz: 0 0 0 \nrpy: -1.5708 -0 -1.5708"]
+"intel_realsense_infra2_optical_joint" -> "intel_realsense_infra2_optical_frame"
+"CHEST_JOINT0_Link" -> "LARM_JOINT0" [label="xyz: 0.04 0.135 0.1015 \nrpy: 0 -0 0"]
+"LARM_JOINT0" -> "LARM_JOINT0_Link"
+"LARM_JOINT0_Link" -> "LARM_JOINT1" [label="xyz: 0 0 0.066 \nrpy: 0 -0 0"]
+"LARM_JOINT1" -> "LARM_JOINT1_Link"
+"LARM_JOINT1_Link" -> "LARM_JOINT2" [label="xyz: 0 0.095 -0.25 \nrpy: 0 -0 0"]
+"LARM_JOINT2" -> "LARM_JOINT2_Link"
+"LARM_JOINT2_Link" -> "LARM_JOINT3" [label="xyz: 0.1805 0 -0.03 \nrpy: 0 -0 0"]
+"LARM_JOINT3" -> "LARM_JOINT3_Link"
+"LARM_JOINT3_Link" -> "LARM_JOINT4" [label="xyz: 0.1495 0 0 \nrpy: 0 -0 0"]
+"LARM_JOINT4" -> "LARM_JOINT4_Link"
+"LARM_JOINT4_Link" -> "LARM_JOINT5" [label="xyz: 0 0 -0.1335 \nrpy: 0 -0 0"]
+"LARM_JOINT5" -> "LARM_JOINT5_Link"
+"LARM_JOINT5_Link" -> "LARM_ft_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"LARM_ft_joint" -> "LARM_ft_link_base"
+"LARM_ft_link_base" -> "LARM_ft_sensor_joint" [label="xyz: 0 0 -0.0333 \nrpy: 3.14159 -0 0"]
+"LARM_ft_sensor_joint" -> "LARM_ft_Sensor_Link"
+"LARM_ft_Sensor_Link" -> "LARM_ft_sensor_offset_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"LARM_ft_sensor_offset_joint" -> "LARM_ft_link"
+"LARM_ft_link" -> "LARM_changer_robot_side_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"LARM_changer_robot_side_joint" -> "LARM_changer_link_base"
+"LARM_changer_link_base" -> "LARM_changer_tool_side_joint" [label="xyz: 0 0 0.039 \nrpy: 0 -0 0"]
+"LARM_changer_tool_side_joint" -> "LARM_changer_link"
+"LARM_changer_link" -> "LARM_ARM_eeff" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"LARM_ARM_eeff" -> "LARM_END_EFFECTOR"
+"LARM_END_EFFECTOR" -> "LARM_ARM_eeff_finger" [label="xyz: -0.02 0 0.075 \nrpy: 0 -0 0"]
+"LARM_ARM_eeff_finger" -> "LARM_END_EFFECTOR_finger"
+"LARM_END_EFFECTOR" -> "LARM_ARM_eeff_grasp" [label="xyz: -0.08 0 0.04 \nrpy: 0 -1.5708 0"]
+"LARM_ARM_eeff_grasp" -> "LARM_END_EFFECTOR_grasp"
+"LARM_END_EFFECTOR_grasp" -> "LARM_ARM_eeff_grasp_end" [label="xyz: 0.045 0 0 \nrpy: 0 -0 0"]
+"LARM_ARM_eeff_grasp_end" -> "LARM_END_EFFECTOR_grasp_end"
+"CHEST_JOINT0_Link" -> "RARM_JOINT0" [label="xyz: 0.04 -0.135 0.1015 \nrpy: 0 -0 0"]
+"RARM_JOINT0" -> "RARM_JOINT0_Link"
+"RARM_JOINT0_Link" -> "RARM_JOINT1" [label="xyz: 0 0 0.066 \nrpy: 0 -0 0"]
+"RARM_JOINT1" -> "RARM_JOINT1_Link"
+"RARM_JOINT1_Link" -> "RARM_JOINT2" [label="xyz: 0 -0.095 -0.25 \nrpy: 0 -0 0"]
+"RARM_JOINT2" -> "RARM_JOINT2_Link"
+"RARM_JOINT2_Link" -> "RARM_JOINT3" [label="xyz: 0.1805 0 -0.03 \nrpy: 0 -0 0"]
+"RARM_JOINT3" -> "RARM_JOINT3_Link"
+"RARM_JOINT3_Link" -> "RARM_JOINT4" [label="xyz: 0.1495 0 0 \nrpy: 0 -0 0"]
+"RARM_JOINT4" -> "RARM_JOINT4_Link"
+"RARM_JOINT4_Link" -> "RARM_JOINT5" [label="xyz: 0 0 -0.1335 \nrpy: 0 -0 0"]
+"RARM_JOINT5" -> "RARM_JOINT5_Link"
+"RARM_JOINT5_Link" -> "RARM_ft_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"RARM_ft_joint" -> "RARM_ft_link_base"
+"RARM_ft_link_base" -> "RARM_ft_sensor_joint" [label="xyz: 0 0 -0.0333 \nrpy: 3.14159 -0 0"]
+"RARM_ft_sensor_joint" -> "RARM_ft_Sensor_Link"
+"RARM_ft_Sensor_Link" -> "RARM_ft_sensor_offset_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"RARM_ft_sensor_offset_joint" -> "RARM_ft_link"
+"RARM_ft_link" -> "RARM_changer_robot_side_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"RARM_changer_robot_side_joint" -> "RARM_changer_link_base"
+"RARM_changer_link_base" -> "RARM_changer_tool_side_joint" [label="xyz: 0 0 0.039 \nrpy: 0 -0 0"]
+"RARM_changer_tool_side_joint" -> "RARM_changer_link"
+"RARM_changer_link" -> "RARM_ARM_eeff" [label="xyz: 0 0 0 \nrpy: 0 -0 0"]
+"RARM_ARM_eeff" -> "RARM_END_EFFECTOR"
+"RARM_END_EFFECTOR" -> "RARM_ARM_eeff_finger" [label="xyz: -0.02 0 0.075 \nrpy: 0 -0 0"]
+"RARM_ARM_eeff_finger" -> "RARM_END_EFFECTOR_finger"
+"RARM_END_EFFECTOR_finger" -> "gaze_ref_joint" [label="xyz: 0.25 0 0 \nrpy: 0 -0 0"]
+"gaze_ref_joint" -> "gaze_ref"
+"RARM_END_EFFECTOR" -> "RARM_ARM_eeff_grasp" [label="xyz: -0.08 0 0.04 \nrpy: 0 -1.5708 0"]
+"RARM_ARM_eeff_grasp" -> "RARM_END_EFFECTOR_grasp"
+"RARM_END_EFFECTOR_grasp" -> "RARM_ARM_eeff_grasp_end" [label="xyz: 0.045 0 0 \nrpy: 0 -0 0"]
+"RARM_ARM_eeff_grasp_end" -> "RARM_END_EFFECTOR_grasp_end"
+"base_link" -> "teleop_ref_joint" [label="xyz: 0.3 0 0 \nrpy: 3.14159 -0 0"]
+"teleop_ref_joint" -> "teleop_ref"
+}
diff --git a/urdf/chonk.pdf b/urdf/chonk.pdf
new file mode 100644
index 0000000..a202665
Binary files /dev/null and b/urdf/chonk.pdf differ
diff --git a/urdf/chonk_pushing.urdf b/urdf/chonk_pushing.urdf
new file mode 100644
index 0000000..01ee60a
--- /dev/null
+++ b/urdf/chonk_pushing.urdf
@@ -0,0 +1,1954 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 30.0
+
+ 1.3962634
+
+ 800
+ 800
+ R8G8B8
+
+
+ 0.1
+ 30
+
+
+ gaussian
+
+ 0.0
+ 0.007
+
+
+
+ true
+ 0.0
+ CAMERA_HEAD_L
+ image_raw
+ camera_info
+ CAMERA_HEAD_L_Link_frame
+ 0.07
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 30.0
+
+ 1.3962634
+
+ 800
+ 800
+ R8G8B8
+
+
+ 0.1
+ 30
+
+
+ gaussian
+
+ 0.0
+ 0.007
+
+
+
+ true
+ 0.0
+ CAMERA_HEAD_R
+ image_raw
+ camera_info
+ CAMERA_HEAD_R_Link_frame
+ 0.07
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.001
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ 1
+
+
+
+
+ 5
+ 5
+ 0.00001
+ 0
+ 1e6
+ 1e2
+ 2.0
+ 0.001
+
+
+
+
+ fl_wheel_link
+ base_link
+ -1 1 0
+
+
+
+ 1
+
+
+ 0.001
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/VelocityJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ 1
+
+
+
+
+ 5
+ 5
+ 0.00001
+ 0
+ 1e6
+ 1e2
+ 2.0
+ 0.001
+
+
+
+
+ fr_wheel_link
+ base_link
+ -1 -1 0
+
+
+
+ 1
+
+
+ 0.001
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/VelocityJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ 1
+
+
+
+
+ 5
+ 5
+ 0.00001
+ 0
+ 1e6
+ 1e2
+ 2.0
+ 0.001
+
+
+
+
+ rl_wheel_link
+ base_link
+ 1 1 0
+
+
+
+ 1
+
+
+ 0.001
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/VelocityJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ 1
+
+
+
+
+ 5
+ 5
+ 0.00001
+ 0
+ 1e6
+ 1e2
+ 2.0
+ 0.001
+
+
+
+
+ rr_wheel_link
+ base_link
+ 1 -1 0
+
+
+
+ 1
+
+
+ 0.001
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/VelocityJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+ parent
+ child_to_parent
+
+
+
+
+
+ 100.0
+ ft_left/raw/data
+ LARM_ft_sensor_joint
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+
+
+ parent
+ child_to_parent
+
+
+
+
+
+ 100.0
+ ft_right/raw/data
+ RARM_ft_sensor_joint
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 10
+ 10
+
+
+
+
+ 0.5
+ 0.2
+ 1e+6
+ 1e3
+ 2
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 10
+ 10
+
+
+
+
+ 0.5
+ 0.2
+ 1e+6
+ 1e3
+ 2
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ /chonk
+
+
+
+
+
+
+
+ true
+ 500
+ donkey_base
+ /chonk/base_pose_ground_truth
+ 0
+ 0 0 0
+ 0 0 0
+
+
+
+
diff --git a/urdf/chonk_pushing.urdf.xacro b/urdf/chonk_pushing.urdf.xacro
new file mode 100644
index 0000000..22cef82
--- /dev/null
+++ b/urdf/chonk_pushing.urdf.xacro
@@ -0,0 +1,144 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ /chonk
+
+
+
+
+
+
+
+
+
+
+ true
+ 500
+ donkey_base
+ /chonk/base_pose_ground_truth
+ 0
+ 0 0 0
+ 0 0 0
+
+
+
+
+
+
diff --git a/urdf/chonk_pushing_chonk.urdf b/urdf/chonk_pushing_chonk.urdf
new file mode 100644
index 0000000..ffae6f3
--- /dev/null
+++ b/urdf/chonk_pushing_chonk.urdf
@@ -0,0 +1,1833 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 30.0
+
+ 1.3962634
+
+ 800
+ 800
+ R8G8B8
+
+
+ 0.1
+ 30
+
+
+ gaussian
+
+ 0.0
+ 0.007
+
+
+
+ true
+ 0.0
+ CAMERA_HEAD_L
+ image_raw
+ camera_info
+ CAMERA_HEAD_L_Link_frame
+ 0.07
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 30.0
+
+ 1.3962634
+
+ 800
+ 800
+ R8G8B8
+
+
+ 0.1
+ 30
+
+
+ gaussian
+
+ 0.0
+ 0.007
+
+
+
+ true
+ 0.0
+ CAMERA_HEAD_R
+ image_raw
+ camera_info
+ CAMERA_HEAD_R_Link_frame
+ 0.07
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.001
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ true
+
+
+ parent
+ child_to_parent
+
+
+
+
+
+ 500.0
+ ft_left/raw/data
+ LARM_ft_sensor_joint
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ true
+
+
+ parent
+ child_to_parent
+
+
+
+
+
+ 500.0
+ ft_right/raw/data
+ RARM_ft_sensor_joint
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+ 0.2
+ 1e+6
+ 1e3
+ 0.1
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+ 0.2
+ 1e+6
+ 1e3
+ 0.1
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ /chonk
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0 0 0 0 0 0
+
+ 1.5009831567151233
+
+ 640
+ 480
+ RGB_INT8
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.007
+
+
+ 1
+ 30
+ 1
+
+
+
+
+ 0 0 0 0 0 0
+
+ 1.5009831567151233
+
+ 640
+ 480
+ L_INT8
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.007
+
+
+ 1
+ 30
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ 1.5009831567151233
+
+ 640
+ 480
+ L_INT8
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.007
+
+
+ 1
+ 30
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ 1.5009831567151233
+
+ 640
+ 480
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.007
+
+
+ 1
+ 30
+ 0
+
+
+
+
+ 30
+ 30
+ 30
+ depth/image_rect_raw
+ depth/camera_info
+ color/image_raw
+ color/camera_info
+ infrared/image_raw
+ infrared/camera_info
+ infrared2/image_raw
+ infrared2/camera_info
+ intel_realsense_color_optical_frame
+ intel_realsense_depth_optical_frame
+ intel_realsense_left_ir_optical_frame
+ intel_realsense_right_ir_optical_frame
+ 0.1
+ 10
+ 1
+ depth/color/points
+ 0.15
+ 10
+
+
+
diff --git a/urdf/chonk_pushing_wholebody.urdf b/urdf/chonk_pushing_wholebody.urdf
new file mode 100644
index 0000000..8929af8
--- /dev/null
+++ b/urdf/chonk_pushing_wholebody.urdf
@@ -0,0 +1,1827 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.001
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 30.0
+
+ 1.3962634
+
+ 800
+ 800
+ R8G8B8
+
+
+ 0.1
+ 30
+
+
+ gaussian
+
+ 0.0
+ 0.007
+
+
+
+ true
+ 0.0
+ CAMERA_HEAD_L
+ image_raw
+ camera_info
+ CAMERA_HEAD_L_Link_frame
+ 0.07
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 30.0
+
+ 1.3962634
+
+ 800
+ 800
+ R8G8B8
+
+
+ 0.1
+ 30
+
+
+ gaussian
+
+ 0.0
+ 0.007
+
+
+
+ true
+ 0.0
+ CAMERA_HEAD_R
+ image_raw
+ camera_info
+ CAMERA_HEAD_R_Link_frame
+ 0.07
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+ 0.0
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+
+
+
+
+
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ true
+
+
+ parent
+ child_to_parent
+
+
+
+
+
+ 500.0
+ ft_left/raw/data
+ LARM_ft_sensor_joint
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ true
+ true
+
+
+ parent
+ child_to_parent
+
+
+
+
+
+ 500.0
+ ft_right/raw/data
+ RARM_ft_sensor_joint
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+ 0.2
+ 1e+6
+ 1e3
+ 0.1
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0
+ 0.2
+ 1e+6
+ 1e3
+ 0.1
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ /chonk
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0 0 0 0 0 0
+
+ 1.5009831567151233
+
+ 640
+ 480
+ RGB_INT8
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.007
+
+
+ 1
+ 30
+ 1
+
+
+
+
+ 0 0 0 0 0 0
+
+ 1.5009831567151233
+
+ 640
+ 480
+ L_INT8
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.007
+
+
+ 1
+ 30
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ 1.5009831567151233
+
+ 640
+ 480
+ L_INT8
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.007
+
+
+ 1
+ 30
+ 0
+
+
+
+
+ 0 0 0 0 0 0
+
+ 1.5009831567151233
+
+ 640
+ 480
+
+
+ 0.1
+ 100
+
+
+ gaussian
+ 0.0
+ 0.007
+
+
+ 1
+ 30
+ 0
+
+
+
+
+ 30
+ 30
+ 30
+ depth/image_rect_raw
+ depth/camera_info
+ color/image_raw
+ color/camera_info
+ infrared/image_raw
+ infrared/camera_info
+ infrared2/image_raw
+ infrared2/camera_info
+ intel_realsense_color_optical_frame
+ intel_realsense_depth_optical_frame
+ intel_realsense_left_ir_optical_frame
+ intel_realsense_right_ir_optical_frame
+ 0.1
+ 10
+ 1
+ depth/color/points
+ 0.15
+ 10
+
+
+
diff --git a/urdf/meshes/end_effector.stl b/urdf/meshes/end_effector.stl
new file mode 100755
index 0000000..f39b3dc
Binary files /dev/null and b/urdf/meshes/end_effector.stl differ
diff --git a/urdf/meshes/end_effector_v1.stl b/urdf/meshes/end_effector_v1.stl
new file mode 100644
index 0000000..2205316
Binary files /dev/null and b/urdf/meshes/end_effector_v1.stl differ
diff --git a/urdf/pushing_tool.xacro b/urdf/pushing_tool.xacro
new file mode 100644
index 0000000..fa4eaa9
--- /dev/null
+++ b/urdf/pushing_tool.xacro
@@ -0,0 +1,131 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 10
+ 10
+
+
+
+
+ 0.5
+ 0.2
+ 1e+6
+ 1e3
+ 2
+ 0.01
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/world/chonk.world b/world/chonk.world
new file mode 100644
index 0000000..2cd5d4c
--- /dev/null
+++ b/world/chonk.world
@@ -0,0 +1,496 @@
+
+
+
+
+
+
+
+
+ model://ground_plane
+
+
+
+
+ model://sun
+
+
+
+
+
+
+
+
+ 3.88902 -3.3977 2.59294 -0 0.275643 2.34819
+ orbit
+ perspective
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 2 0 0 -0 1.5707963
+
+
+
+ 3.85 2 1.165 0 -0 0
+
+
+ 2.0
+
+ 0.166667
+ 0
+ 0
+ 0.166667
+ 0
+ 0.166667
+
+ 0 0 0 0 -0 0
+
+
+
+
+ 0.3 0.3 0.3
+
+
+ 10000
+
+
+
+ 10
+ 10
+
+
+ 10
+ 10
+ false
+
+
+
+
+ 1e+6
+ 1e+3
+ 0
+ 0.002
+
+
+
+
+
+
+
+
+ 0.3 0.3 0.3
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 -2 0 0 -0 1.5707963
+
+
+
+
+
+
+
+
diff --git a/world/chonk_pick.world b/world/chonk_pick.world
new file mode 100644
index 0000000..f33adff
--- /dev/null
+++ b/world/chonk_pick.world
@@ -0,0 +1,495 @@
+
+
+
+
+
+
+
+
+ model://ground_plane
+
+
+
+
+ model://sun
+
+
+
+
+
+
+
+
+ 3.88902 -3.3977 2.59294 -0 0.275643 2.34819
+ orbit
+ perspective
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 2 0 0 -0 1.5707963
+
+
+
+ 3.85 2 1.165 0 -0 0
+
+
+ 1.2
+
+ 0.166667
+ 0
+ 0
+ 0.166667
+ 0
+ 0.166667
+
+ 0 0 0 0 -0 0
+
+
+
+
+ 0.25 0.3 0.3
+
+
+ 10000
+
+
+
+ 100
+ 100
+
+
+ 100
+ 100
+ false
+
+
+
+
+ 1e+5
+ 1e+2
+ 2
+ 0.01
+
+
+
+
+
+
+
+
+ 0.25 0.3 0.3
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 -2 0 0 -0 1.5707963
+
+
+
+
+
+
+
diff --git a/world/chonk_pick_obstable.world b/world/chonk_pick_obstable.world
new file mode 100644
index 0000000..9d39f1d
--- /dev/null
+++ b/world/chonk_pick_obstable.world
@@ -0,0 +1,550 @@
+
+
+
+
+
+
+
+
+ model://ground_plane
+
+
+
+
+ model://sun
+
+
+
+
+
+
+
+
+ 3.88902 -3.3977 2.59294 -0 0.275643 2.34819
+ orbit
+ perspective
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 2 0 0 -0 1.5707963
+
+
+
+ 3.85 2 1.165 0 -0 0
+
+
+ 2.0
+
+ 1.66667
+ 0
+ 0
+ 1.66667
+ 0
+ 1.66667
+
+ 0 0 0 0 -0 0
+
+
+
+
+ 0.25 0.3 0.3
+
+
+ 10000
+
+
+
+ 10
+ 10
+
+
+ 10
+ 10
+ false
+
+
+
+
+ 1e+6
+ 1e+3
+ 0
+ 0.002
+
+
+
+
+
+
+
+
+ 0.25 0.3 0.3
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 -2 0 0 -0 1.5707963
+
+
+
+ 4 0 1 0 -0 0
+
+
+ 1000
+
+ 145.833
+ 0
+ 0
+ 145.833
+ 0
+ 125
+
+ 0 0 0 0 -0 0
+
+
+
+
+ 1
+ 2
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+ 2
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+
+
+
diff --git a/world/chonk_pick_operational.world b/world/chonk_pick_operational.world
new file mode 100644
index 0000000..3f763f8
--- /dev/null
+++ b/world/chonk_pick_operational.world
@@ -0,0 +1,549 @@
+
+
+
+
+
+
+
+
+ model://ground_plane
+
+
+
+
+ model://sun
+
+
+
+
+
+
+
+
+ 3.88902 -3.3977 2.59294 -0 0.275643 2.34819
+ orbit
+ perspective
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 2 0 0 -0 1.5707963
+
+
+
+ 3.85 2 1.165 0 -0 0
+
+
+ 0.3
+
+ 0.166667
+ 0
+ 0
+ 0.166667
+ 0
+ 0.166667
+
+ 0 0 0 0 -0 0
+
+
+
+
+ 0.25 0.3 0.3
+
+
+ 1000000
+
+
+
+ 10
+ 10
+
+
+ 10
+ 10
+ false
+
+
+
+
+ 1e+6
+ 1e+3
+ 0.01
+ 0.005
+
+
+
+
+
+
+
+
+ 0.25 0.3 0.3
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 -2 0 0 -0 1.5707963
+
+
+
+
+
+
+
diff --git a/world/chonk_pick_operational_experiment.world b/world/chonk_pick_operational_experiment.world
new file mode 100644
index 0000000..153e376
--- /dev/null
+++ b/world/chonk_pick_operational_experiment.world
@@ -0,0 +1,549 @@
+
+
+
+
+
+
+
+
+ model://ground_plane
+
+
+
+
+ model://sun
+
+
+
+
+
+
+
+
+ 3.88902 -3.3977 2.59294 -0 0.275643 2.34819
+ orbit
+ perspective
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 2 2.7 0 0 -0 0
+
+
+
+ 2 2.5 1.165 0 -0 0
+
+
+ 0.3
+
+ 0.166667
+ 0
+ 0
+ 0.166667
+ 0
+ 0.166667
+
+ 0 0 0 0 -0 0
+
+
+
+
+ 0.3 0.25 0.3
+
+
+ 10000
+
+
+
+ 10
+ 10
+
+
+ 10
+ 10
+ false
+
+
+
+
+ 1e+6
+ 1e+3
+ 0
+ 0.002
+
+
+
+
+
+
+
+
+ 0.3 0.25 0.3
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 3 0 0 0 -0 1.5707963
+
+
+
+
+
+
+
diff --git a/world/chonk_pick_sensorAD_obstacle.world b/world/chonk_pick_sensorAD_obstacle.world
new file mode 100644
index 0000000..a4ef308
--- /dev/null
+++ b/world/chonk_pick_sensorAD_obstacle.world
@@ -0,0 +1,551 @@
+
+
+
+
+
+
+
+
+ model://ground_plane
+
+
+
+
+ model://sun
+
+
+
+
+
+
+
+
+ 3.88902 -3.3977 2.59294 -0 0.275643 2.34819
+ orbit
+ perspective
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 2 0 0 -0 1.5707963
+
+
+
+ 3.85 2 1.165 0 -0 0
+
+
+ 1.2
+
+ 0.166667
+ 0
+ 0
+ 0.166667
+ 0
+ 0.166667
+
+ 0 0 0 0 -0 0
+
+
+
+
+ 0.25 0.3 0.3
+
+
+ 10000
+
+
+
+ 100
+ 100
+
+
+ 100
+ 100
+ false
+
+
+
+
+ 1e+5
+ 1e+2
+ 2
+ 0.01
+
+
+
+
+
+
+
+
+ 0.25 0.3 0.3
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 -2 0 0 -0 1.5707963
+
+
+
+ 4 0 1 0 -0 0
+
+
+ 1000
+
+ 145.833
+ 0
+ 0
+ 145.833
+ 0
+ 125
+
+ 0 0 0 0 -0 0
+
+
+
+
+ 1
+ 2
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+ 2
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+
+
+
+
diff --git a/world/chonk_pick_turn.world b/world/chonk_pick_turn.world
new file mode 100644
index 0000000..13e714e
--- /dev/null
+++ b/world/chonk_pick_turn.world
@@ -0,0 +1,689 @@
+
+
+
+
+
+
+
+
+ model://ground_plane
+
+
+
+
+ model://sun
+
+
+
+
+
+
+
+
+ 3.88902 -3.3977 2.59294 -0 0.275643 2.34819
+ orbit
+ perspective
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 2.5 3.65 0 0 -0 0
+
+
+
+ 2.5 3.5 1.165 0 -0 0
+
+
+ 1.2
+
+ 0.166667
+ 0
+ 0
+ 0.166667
+ 0
+ 0.166667
+
+ 0 0 0 0 -0 0
+
+
+
+
+ 0.3 0.25 0.3
+
+
+ 10000
+
+
+
+ 100
+ 100
+
+
+ 100
+ 100
+ false
+
+
+
+
+ 1e+5
+ 1e+2
+ 2
+ 0.01
+
+
+
+
+
+
+
+
+ 0.3 0.25 0.3
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 -2 0 0 -0 1.5707963
+
+
+
+
+
+
+
diff --git a/world/chonk_pick_turn_obstacle.world b/world/chonk_pick_turn_obstacle.world
new file mode 100644
index 0000000..a5a2680
--- /dev/null
+++ b/world/chonk_pick_turn_obstacle.world
@@ -0,0 +1,745 @@
+
+
+
+
+
+
+
+
+ model://ground_plane
+
+
+
+
+ model://sun
+
+
+
+
+
+
+
+
+ 3.88902 -3.3977 2.59294 -0 0.275643 2.34819
+ orbit
+ perspective
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 2.5 3.15 0 0 -0 0
+
+
+
+ 2.5 3 1.165 0 -0 0
+
+
+ 1.2
+
+ 0.166667
+ 0
+ 0
+ 0.166667
+ 0
+ 0.166667
+
+ 0 0 0 0 -0 0
+
+
+
+
+ 0.3 0.25 0.3
+
+
+ 10000
+
+
+
+ 100
+ 100
+
+
+ 100
+ 100
+ false
+
+
+
+
+ 1e+5
+ 1e+2
+ 2
+ 0.01
+
+
+
+
+
+
+
+
+ 0.3 0.25 0.3
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+
+ 1
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+ 0.6
+ 0.6
+
+
+
+
+
+
+
+
+
+
+ 10
+
+
+ 0 0 1 0 -0 0
+
+
+ 1.5 0.8 0.03
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 -0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -0.68 0.38 0.5 0 -0 0
+
+
+ 0.02
+ 1
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+ 4 -2 0 0 -0 1.5707963
+
+
+
+
+
+ 4 0 1 0 -0 0
+
+
+ 1000
+
+ 145.833
+ 0
+ 0
+ 145.833
+ 0
+ 125
+
+ 0 0 0 0 -0 0
+
+
+
+
+ 1
+ 2
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1
+ 2
+
+
+
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+
+
diff --git a/world/chonk_push.world b/world/chonk_push.world
new file mode 100644
index 0000000..54c8fbd
--- /dev/null
+++ b/world/chonk_push.world
@@ -0,0 +1,106 @@
+
+
+
+
+
+
+
+
+ model://ground_plane
+
+
+
+
+ model://sun
+
+
+
+
+
+
+
+
+ 3.88902 -3.3977 2.59294 -0 0.275643 2.34819
+ orbit
+ perspective
+
+
+
+
+
+
+
+
+
+
+
+
+
+