Skip to content
GitLab
Commit 8ca3dbbb authored by Luca's avatar Luca
Browse files

Position Direct Control has been implemented

parent 81b862e5
Hide whitespace changes
Inline Side-by-side
.idea/misc.xml 0 → 100644
View file @ 8ca3dbbb
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
</project>
\ No newline at end of file
.idea/modules.xml 0 → 100644
View file @ 8ca3dbbb
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/velocityController.iml" filepath="$PROJECT_DIR$/.idea/velocityController.iml" />
</modules>
</component>
</project>
\ No newline at end of file
.idea/vcs.xml 0 → 100644
View file @ 8ca3dbbb
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="$PROJECT_DIR$" vcs="Git" />
</component>
</project>
\ No newline at end of file
.idea/velocityController.iml 0 → 100644
View file @ 8ca3dbbb
<?xml version="1.0" encoding="UTF-8"?>
<module classpath="CMake" type="CPP_MODULE" version="4" />
\ No newline at end of file
.idea/workspace.xml 0 → 100644
View file @ 8ca3dbbb
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="CMakeRunConfigurationManager" shouldGenerate="true" shouldDeleteObsolete="true">
<generated>
<config projectName="velocityController" targetName="velocityController" />
<config projectName="velocityController" targetName="uninstall" />
<config projectName="velocityController" targetName="copy_lua_in_build" />
</generated>
</component>
<component name="CMakeSettings" AUTO_RELOAD="true">
<configurations>
<configuration PROFILE_NAME="Debug" ENABLED="true" GENERATION_DIR="build" CONFIG_NAME="Debug" GENERATION_OPTIONS="-G &quot;Unix Makefiles&quot;" />
</configurations>
</component>
<component name="ChangeListManager">
<list default="true" id="f49e54e8-1c2f-4aa2-b4ea-b05666a56758" name="Changes" comment="">
<change beforePath="$PROJECT_DIR$/CMakeLists.txt" beforeDir="false" afterPath="$PROJECT_DIR$/CMakeLists.txt" afterDir="false" />
<change beforePath="$PROJECT_DIR$/velocityControllerModule.cpp" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/velocityControllerThread.cpp" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/velocityControllerThread.h" beforeDir="false" />
</list>
<option name="SHOW_DIALOG" value="false" />
<option name="HIGHLIGHT_CONFLICTS" value="true" />
<option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
<option name="LAST_RESOLUTION" value="IGNORE" />
</component>
<component name="ClangdSettings">
<option name="formatViaClangd" value="false" />
</component>
<component name="ExecutionTargetManager" SELECTED_TARGET="CMakeBuildProfile:Debug" />
<component name="Git.Settings">
<option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$" />
</component>
<component name="MarkdownSettingsMigration">
<option name="stateVersion" value="1" />
</component>
<component name="ProjectApplicationVersion">
<option name="ide" value="CLion" />
<option name="majorVersion" value="2021" />
<option name="minorVersion" value="3.2" />
</component>
<component name="ProjectId" id="24EpOlZPkH8TS42hciXY2kBq1mx" />
<component name="ProjectLevelVcsManager" settingsEditedManually="true" />
<component name="ProjectViewState">
<option name="hideEmptyMiddlePackages" value="true" />
<option name="showLibraryContents" value="true" />
</component>
<component name="PropertiesComponent">
<property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
<property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
<property name="RunOnceActivity.cidr.known.project.marker" value="true" />
<property name="WebServerToolWindowFactoryState" value="false" />
<property name="cf.first.check.clang-format" value="false" />
<property name="cidr.known.project.marker" value="true" />
<property name="cmake.loaded.for.project" value="true" />
<property name="settings.editor.selected.configurable" value="CMakeSettings" />
</component>
<component name="RunManager" selected="CMake Application.Unnamed">
<configuration name="Unnamed" type="CLionNativeAppRunConfigurationType" nameIsGenerated="true" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" PASS_PARENT_ENVS_2="true" version="1">
<method v="2">
<option name="CLION.COMPOUND.BUILD" enabled="true" />
</method>
</configuration>
<configuration name="Unnamed" type="CMakeRunConfiguration" factoryName="Application" nameIsGenerated="true" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" WORKING_DIR="file://$PROJECT_DIR$" PASS_PARENT_ENVS_2="true" CONFIG_NAME="Debug" RUN_PATH="$PROJECT_DIR$/CMakeLists.txt" EXPLICIT_BUILD_TARGET_NAME="all">
<method v="2">
<option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
</method>
</configuration>
<configuration name="copy_lua_in_build" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="velocityController" TARGET_NAME="copy_lua_in_build" CONFIG_NAME="Debug">
<method v="2">
<option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
</method>
</configuration>
<configuration name="uninstall" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="velocityController" TARGET_NAME="uninstall" CONFIG_NAME="Debug">
<method v="2">
<option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
</method>
</configuration>
<configuration name="velocityController" type="CMakeRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="velocityController" TARGET_NAME="velocityController" CONFIG_NAME="Debug" RUN_TARGET_PROJECT_NAME="velocityController" RUN_TARGET_NAME="velocityController">
<method v="2">
<option name="com.jetbrains.cidr.execution.CidrBuildBeforeRunTaskProvider$BuildBeforeRunTask" enabled="true" />
</method>
</configuration>
<list>
<item itemvalue="Makefile Application.Unnamed" />
<item itemvalue="CMake Application.Unnamed" />
<item itemvalue="CMake Application.copy_lua_in_build" />
<item itemvalue="CMake Application.uninstall" />
<item itemvalue="CMake Application.velocityController" />
</list>
</component>
<component name="SpellCheckerSettings" RuntimeDictionaries="0" Folders="0" CustomDictionaries="0" DefaultDictionary="application-level" UseSingleDictionary="true" transferred="true" />
<component name="TaskManager">
<task active="true" id="Default" summary="Default task">
<changelist id="f49e54e8-1c2f-4aa2-b4ea-b05666a56758" name="Changes" comment="" />
<created>1643205574532</created>
<option name="number" value="Default" />
<option name="presentableId" value="Default" />
<updated>1643205574532</updated>
<workItem from="1643205581629" duration="7000" />
<workItem from="1643208757006" duration="10000" />
<workItem from="1643232050805" duration="13023000" />
</task>
<servers />
</component>
<component name="Vcs.Log.Tabs.Properties">
<option name="TAB_STATES">
<map>
<entry key="MAIN">
<value>
<State />
</value>
</entry>
</map>
</option>
</component>
</project>
\ No newline at end of file
CMakeLists.txt
View file @ 8ca3dbbb
# Copyright: 2012 iCub Facility, Istituto Italiano di Tecnologia
# Author: Lorenzo Natale
# Author: Luca Garello
# CopyPolicy: Released under the terms of the GNU GPL v2.0.
#
cmake_minimum_required(VERSION 3.5)
project(velocityController)
find_package(YARP)
file(GLOB source *.cpp)
file(GLOB header *.h)
find_package(YARP REQUIRED)
find_package(ICUBcontrib REQUIRED)
list(APPEND CMAKE_MODULE_PATH ${ICUBCONTRIB_MODULE_PATH})
# We need these since we install in ICUBcontrib
include(ICUBcontribHelpers)
include(ICUBcontribOptions)
icubcontrib_set_default_prefix()
source_group("Source Files" FILES ${source})
source_group("Header Files" FILES ${header})
# add the folders with app and module
add_subdirectory(module)
add_subdirectory(app)
add_executable(velocityController ${source} ${header})
target_link_libraries(velocityController ${YARP_LIBRARIES})
icubcontrib_add_uninstall_target()
install(TARGETS velocityController DESTINATION bin)
#get_cmake_property(_variableNames VARIABLES)
#list (SORT _variableNames)
#foreach (_variableName ${_variableNames})
# message(STATUS "${_variableName}=${${_variableName}}")
#endforeach()
app/CMakeLists.txt 0 → 100755
View file @ 8ca3dbbb
# Copyright: (C) 2014 iCub Facility - Istituto Italiano di Tecnologia
# Authors: Alessandro Roncone
# CopyPolicy: Released under the terms of the GNU GPL v2.0.
cmake_minimum_required(VERSION 3.5)
file(GLOB scripts ${CMAKE_CURRENT_SOURCE_DIR}/scripts/*.xml)
file(GLOB scripts_template ${CMAKE_CURRENT_SOURCE_DIR}/scripts/*.xmltemplate)
file(GLOB conf_ini ${CMAKE_CURRENT_SOURCE_DIR}/conf/*.ini ${CMAKE_CURRENT_SOURCE_DIR}/conf/*.lua)
file(GLOB conf_svg ${CMAKE_CURRENT_SOURCE_DIR}/conf/drawings/*.svg)
file(GLOB lua_bin ${CMAKE_CURRENT_SOURCE_DIR}/scripts/lua/*.lua)
yarp_install(FILES ${conf_ini} DESTINATION ${ICUBCONTRIB_CONTEXTS_INSTALL_DIR}/${PROJECTNAME})
yarp_install(FILES ${conf_svg} DESTINATION ${ICUBCONTRIB_CONTEXTS_INSTALL_DIR}/${PROJECTNAME}/drawings)
yarp_install(FILES ${scripts_template} DESTINATION ${ICUBCONTRIB_APPLICATIONS_TEMPLATES_INSTALL_DIR})
yarp_install(FILES ${scripts} DESTINATION ${ICUBCONTRIB_APPLICATIONS_INSTALL_DIR})
add_custom_target(copy_lua_in_build ALL)
foreach(_bin IN ITEMS ${lua_bin})
get_filename_component(_bin_name ${_bin} NAME)
add_custom_command(TARGET copy_lua_in_build POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy ${_bin} ${CMAKE_BINARY_DIR}/bin/${CMAKE_CFG_INTDIR}/${_bin_name}
COMMENT "Copying ${_bin} to ${CMAKE_BINARY_DIR}/bin/${CMAKE_CFG_INTDIR}/${_bin_name}")
endforeach()
install(PROGRAMS ${lua_bin} DESTINATION bin)
app/scripts/velocityController.xml 0 → 100644
View file @ 8ca3dbbb
<application>
<name>velocityController</name>
<dependencies>
</dependencies>
<module>
<name>gazebo</name>
<parameters></parameters>
<node>localhost</node>
<stdio></stdio>
<tag>gazebo</tag>
</module>
<module>
<name>yarprobotinterface</name>
<parameters>--context simCartesianControl --config no_legs.xml</parameters>
<dependencies>
<port timeout="20">/icubSim/torso/state:o</port>
<port timeout="20">/icubSim/left_arm/state:o</port>
<port timeout="20">/icubSim/right_arm/state:o</port>
</dependencies>
<environment>YARP_FORWARD_LOG_ENABLE=1</environment>
<ensure>
<wait when="stop">5</wait>
</ensure>
<node>localhost</node>
</module>
<module>
<name>iKinCartesianSolver</name>
<parameters>--context simCartesianControl --part right_arm</parameters>
<dependencies>
<port timeout="20">/icubSim/torso/state:o</port>
<port timeout="20">/icubSim/right_arm/state:o</port>
</dependencies>
<environment>YARP_FORWARD_LOG_ENABLE=1</environment>
<node>localhost</node>
</module>
<module>
<name>yarpdatadumper</name>
<parameters> --name /data/rightArm --rxTime</parameters>
<node>localhost</node>
<stdio></stdio>
<tag>yarpdatadumper</tag>
</module>
<!-- ################################################################## -->
<connection>
<from>/icubSim/cartesianController/right_arm/state:o</from>
<to>/data/rightArm</to>
<protocol>udp</protocol>
</connection>
</application>
module/CMakeLists.txt 0 → 100644
View file @ 8ca3dbbb
# Copyright: (C) 2014 iCub Facility - Istituto Italiano di Tecnologia
# Author: Alessandro Roncone
# CopyPolicy: Released under the terms of the GNU GPL v2.0.
cmake_minimum_required(VERSION 3.5)
project(velocityController)
file(GLOB source *.cpp)
file(GLOB header *.h)
source_group("Source Files" FILES ${source})
source_group("Header Files" FILES ${header})
add_executable(velocityController ${source} ${header})
target_link_libraries(velocityController ${YARP_LIBRARIES})
install(TARGETS velocityController DESTINATION bin)
velocityControllerThread.cpp → module/velocityControllerThread.cpp
View file @ 8ca3dbbb
......@@ -24,7 +24,7 @@
//#include <yarp/dev/GazeControl.h>
//#include <yarp/dev/PolyDriver.h>
#define TASK_PERIOD 0.15 // [s] delta_time between two successive trajectory steps
#define TASK_PERIOD 0.01 // [s] delta_time between two successive trajectory steps
#define CTRL_THREAD_PER 0.005 // [s]
#define MAX_TORSO_PITCH 30.0 // [deg]
......@@ -46,8 +46,8 @@ CtrlThread::CtrlThread(const double period, yarp::os::ResourceFinder rf) : Perio
CtrlThread::~CtrlThread(){};
bool CtrlThread::threadInit() {
std::string robotName = rf.find("robot").asString();
std::string partName = rf.find("part").asString();
robotName = rf.find("robot").asString();
partName = rf.find("part").asString();
enable_torso = rf.find("torso").asBool();
enable_gaze = rf.find("gaze").asBool();
std::string file_path = rf.find("file_path").asString();
......@@ -135,7 +135,7 @@ bool CtrlThread::threadInit() {
fprintf(stdout,"info = %s\n",info.toString().c_str());
// register the event, attaching the callback
icart->registerEvent(*this);
//icart->registerEvent(*this);
A.resize(3);
B.resize(3);
......@@ -143,30 +143,78 @@ bool CtrlThread::threadInit() {
od.resize(4);
x_dot.resize(3);
o_dot.resize(4);
od[0] = 0.0;
od[1] = 0.0;
od[2] = 1.0;
od[3] = M_PI;
A[0] = -0.157;
A[1] = 0.275;
A[2] = 0.324;
B[0] = -0.253;
B[1] = 0.256;
B[2] = -0.026;
//
A_fro.resize(3);
B_fro.resize(3);
od_fro.resize(4);
A_sag.resize(3);
B_sag.resize(3);
od_sag.resize(4);
A_obl.resize(3);
B_obl.resize(3);
od_obl.resize(4);
A_mat.resize(3,3);
B_mat.resize(3,3);
od_mat.resize(3,4);
//
//od[0] = 0.0;
//od[1] = 1.0;
//od[2] = 0.0;
//od[3] = M_PI;
// sagittale
// od[0] = 0.22;
// od[1] = 0.314;
// od[2] = -0.923;
// od[3] = M_PI;
// cerchio (obliquo)
// od[0] = -0.276;
// od[1] = -0.614;
// od[2] = 0.739;
// od[3] = 2.098;
// od[0] = 0.005366;
// od[1] = 0.424294;
// od[2] = -0.905508;
// od[3] = 2.982768;
// palmo verticale verso centro 0 0 1 M_PI
//palmo orizzontale verso basso 0 1 0 M_PI
// Frontale
B_fro = { -0.25, 0.0, 0.1 };
A_fro = { -0.25, 0.3, 0.1 };
od_fro = {0 , 0 , 1 , M_PI };
A_fro = { -0.16, 0.01, 0.00 }; //new berry
B_fro = { -0.33, 0.05, 0.21 };
od_fro = {0 , 0 , 1 , M_PI };
// Sagittale
A_sag = { -0.265, 0.124, 0.249 }; // questo OKKKK
B_sag = { -0.265, 0.150, 0.150 };
od_sag = {0 , 1 , 0 , M_PI };
A_sag = { -0.104, 0.373, 0.336 }; //new berry
B_sag = { -0.360, 0.092, 0.220 };
od_sag = {0 , 1 , 0 , M_PI };
// Obliquo
A_obl = { -0.218, 0.396, 0.143 };
B_obl = { -0.313, 0.152, -0.005 };
od_obl = {0.005366 , 0.424294 , -0.905508 , 2.982768 };
//
A = A_sag;
B = B_sag;
od = od_sag;
distance = sqrt(pow(B[0] - A[0], 2) + pow(B[1] - A[1], 2) + pow(B[2] - A[2], 2));
xd = A;
//to get the direction of the segment A->B we compute the simple element-wise B-A
AB.resize(3);
AB[0] = B[0] - A[0];
AB[1] = B[1] - A[1];
AB[2] = B[2] - A[2];
/* set the position controller */
Property optionJoints;
......@@ -175,11 +223,6 @@ bool CtrlThread::threadInit() {
optionJoints.put("remote", "/" + robotName + "/" + partName); //where we connect to
optionJoints.put("writeStrict","on");
// if (!robotDevice.open(optionJoints)) {
// printf("Device not available. Here are the known devices:\n");
// printf("%s", Drivers::factory().toString().c_str());
// }
robotDevice.open(optionJoints);
if (!robotDevice.isValid()) {
......@@ -188,10 +231,6 @@ bool CtrlThread::threadInit() {
return 0;
}
// bool ok;
// ok = this->robotDevice.view(pos);
// ok = ok && this->robotDevice.view(encs);
bool ok;
ok = robotDevice.view(encs);
ok = robotDevice.view(pos);
......@@ -207,18 +246,13 @@ bool CtrlThread::threadInit() {
yDebug("njoints = %d", njoints);
encoders.resize(njoints);
// //changing the control mode
// for (int i = 0; i < 16; i++) {
// ictrl->setControlMode(i, VOCAB_CM_POSITION_DIRECT);
// }
while(!encs->getEncoders(encoders.data()))
{
Time::delay(0.1);
printf(".");
}
trial_n = 0;
trial_n = 1;
return true;
}
......@@ -229,23 +263,20 @@ void CtrlThread::afterStart(bool s) {
else
fprintf(stdout,"Thread did not start\n");
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void CtrlThread::run() {
//icart->setPosePriority("position");
//wait for new commands on the port
fprintf(stdout,"NEW ACTION:\n");
printf("Going to start position\n\n");
icart->goToPose(A, od, 4);
icart->goToPose(A, od,1.5);
Time::delay(4);
//plan trajectory
planAndPlayTrajectory();
Time::delay(4);
planTrajectory();
playTrajectory();
Time::delay(2);
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void CtrlThread::threadRelease() {
// we require an immediate stop
......@@ -262,6 +293,11 @@ void CtrlThread::threadRelease() {
// ask the solver to compute the joint angles that allow to reach that position
void CtrlThread::planTrajectory(){
//to get the direction of the segment A->B we compute the simple element-wise B-A
AB[0] = B[0] - A[0];
AB[1] = B[1] - A[1];
AB[2] = B[2] - A[2];
trajectory.resize(time.length(),njoints+3);//+3 for the torso
Vector q_start;
......@@ -273,7 +309,7 @@ void CtrlThread::planTrajectory(){
encs->getEncoders(q_start.data());
printf("START PLANNING: \n");
printf("PLANNING: \n");
for (int i = 0; i < time.length(); i++) {
xd[0] = A[0] + AB[0] * (time[i] / distance);
xd[1] = A[1] + AB[1] * (time[i] / distance);
......@@ -286,56 +322,85 @@ void CtrlThread::planTrajectory(){
trajectory.setRow(i, q_final);
q_start = q_final;
if ( (int((float(i+1)/time.length())*1000)) % 200 == 0 ){
yInfo("progress: %i ", int((float(i+1)/time.length())*100) );
}
}
}
void CtrlThread::playTrajectory(){
//get number of actuated joints
int actuated_joints;
//idir->getAxes(&actuated_joints);
pos->getAxes(&actuated_joints);
printf("actuated joints: %i \n", actuated_joints);
//set all joints in position direct mode
for (int i = 0; i < actuated_joints; i++) {
ictrl->setControlMode(i, VOCAB_CM_POSITION_DIRECT);
//ictrl->setControlMode(i, VOCAB_CM_POSITION);
}
// read encoders
//yInfo("the plan is: \n\n%s", trajectory.toString().c_str());
encs->getEncoders(encoders.data());
yInfo("start from: %s", encoders.toString().c_str());
std::array<int,4> ar = {0,1,2,3};
// Vector to_control;
// to_control.resize(4);
// to_control[0] = 0;
// to_control[1] = 1;
// to_control[2] = 2;
// to_control[3] = 3;
for (size_t i = 0; i < trajectory.rows(); i++) {
Vector command = trajectory.getRow(i).subVector(3,7);
yInfo("going to: %s", command.toString().c_str());
//pos->positionMove(command.data());
idir->setPositions(command.data());
Time::delay(0.5);
}
//pos->positionMove(trajectory.getRow(trajectory.rows()).subVector(3,7).data());
Time::delay(10);
yInfo("azione finita da 10 sec: Cambio control mode");
//set all joints in position direct mode (to use the cartesian solver)
// for (int i = 0; i < actuated_joints; i++) {
// ictrl->setControlMode(i, VOCAB_CM_POSITION_DIRECT);
// }
Time::delay(5);
Matrix velocityReport(trajectory.rows(), 3);
Vector command,prev_command;
command.resize(trajectory.cols());
prev_command.resize(trajectory.cols());
//print planned trajectory
yInfo("%s", trajectory.toString().c_str());
encs->getEncoders(encoders.data());
yInfo("from: | %s | ", encoders.toString().c_str());
prev_command = trajectory.getRow(0);
cout << "Checking that target joints positions are safe" << endl;
for (int i = 0; i < trajectory.rows(); i++) {
command = trajectory.getRow(i); //10 joints
for (int j = 0; j < trajectory.cols(); j++) {
//fix the single encoder value for each row by setting the previous value
if (abs(command[j]-prev_command[j]) > 5.0) {command[j] = prev_command[j];}
}
//update
trajectory.setRow(i, command);
prev_command = command;
}
//get number of actuated joints
int actuated_joints;
//idir->getAxes(&actuated_joints);
pos->getAxes(&actuated_joints);
//set all joints in position direct mode
for (int i = 0; i < actuated_joints; i++) {
//ictrl->setControlMode(i, VOCAB_CM_POSITION);
ictrl->setControlMode(i, VOCAB_CM_POSITION_DIRECT);
}
Network::connect("/" + robotName + "/cartesianController/right_arm/state:o","/data/rightArm");
for (size_t i = 0; i < trajectory.rows(); i++) {
t = Time::now();
//ho 10 colonne di traiettoria, 3 torso + 4 braccio + 3 polso
command = trajectory.getRow(i).subVector(3,9);
// wee keep constanst the wrist orientation
command.setSubvector(4, trajectory.getRow(0).subVector(7,9));