Skip to content

Commit 6be319c

Browse files
juanjqojuanjqo
authored
line-to-line-angle-constraint example (#13)
* Added a minimal example to test the line-to-line-angle Jacobian and its residual. * Create README.md * Update README.md
1 parent 66a3974 commit 6be319c

File tree

4 files changed

+221
-0
lines changed

4 files changed

+221
-0
lines changed

‎cmake/vrep_interface_tests/line_to_line_angle_jacobian/CMakeLists.txt‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
cmake_minimum_required(VERSION 3.1)
2+
3+
project(test_dynamic_conic_constraint)
4+
5+
find_package (Threads REQUIRED)
6+
7+
set (CMAKE_CXX_STANDARD 11)
8+
find_package (Threads REQUIRED)
9+
FIND_PACKAGE(Eigen3 REQUIRED)
10+
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
11+
12+
add_definitions(-DIL_STD)
13+
add_definitions(-DNON_MATLAB_PARSING)
14+
add_definitions(-DMAX_EXT_API_CONNECTIONS=255)
15+
add_definitions(-DDO_NOT_USE_SHARED_MEMORY)
16+
17+
18+
19+
add_executable(test_dynamic_conic_constraint
20+
test_dynamic_conic_constraint.cpp
21+
)
22+
23+
24+
TARGET_LINK_LIBRARIES(test_dynamic_conic_constraint
25+
pthread
26+
dqrobotics
27+
dqrobotics-interface-vrep
28+
)
29+

‎cmake/vrep_interface_tests/line_to_line_angle_jacobian/README.md‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
2+
To disable the residual in `test_dynamic_conic_constraint.cpp` change the line
3+
4+
```shell
5+
const bool USE_RESIDUAL = true;
6+
```
7+
8+
to
9+
10+
```shell
11+
const bool USE_RESIDUAL = false;
12+
```
13+
14+
![ezgif com-gif-maker](https://user-images.githubusercontent.com/23158313/201058867-dcd85e4b-7d35-494c-b5b6-5337c066f4aa.gif)

‎cmake/vrep_interface_tests/line_to_line_angle_jacobian/test_dynamic_conic_constraint.cpp‎

Lines changed: 178 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,178 @@
1+
/**
2+
(C) Copyright 2022 DQ Robotics Developers
3+
This file is part of DQ Robotics.
4+
DQ Robotics is free software: you can redistribute it and/or modify
5+
it under the terms of the GNU Lesser General Public License as published by
6+
the Free Software Foundation, either version 3 of the License, or
7+
(at your option) any later version.
8+
DQ Robotics is distributed in the hope that it will be useful,
9+
but WITHOUT ANY WARRANTY; without even the implied warranty of
10+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11+
GNU Lesser General Public License for more details.
12+
You should have received a copy of the GNU Lesser General Public License
13+
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
14+
Contributors:
15+
- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp)
16+
17+
Instructions:
18+
Prerequisites:
19+
- dqrobotics
20+
- dqrobotics-vrep-interface
21+
22+
1) Open the CoppeliaSim scene test_dynamic_conic_constraint.ttt
23+
2) Compile, run and enjoy!
24+
*/
25+
26+
#include <iostream>
27+
#include <Eigen/Dense>
28+
#include <dqrobotics/DQ.h>
29+
#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
30+
#include <thread>
31+
#include <dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h>
32+
#include <dqrobotics/solvers/DQ_QPOASESSolver.h>
33+
#include <dqrobotics/robot_control/DQ_ClassicQPController.h>
34+
#include<dqrobotics/utils/DQ_Constants.h>
35+
#include <dqrobotics/utils/DQ_Geometry.h>
36+
37+
38+
using namespace Eigen;
39+
using namespace DQ_robotics;
40+
41+
int main()
42+
{
43+
const bool USE_RESIDUAL = true;
44+
45+
DQ_VrepInterface vi;
46+
vi.connect(19997,100,10);
47+
vi.set_synchronous(true);
48+
std::cout << "Starting V-REP simulation..." << std::endl;
49+
vi.start_simulation();
50+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
51+
int iterations = 15000; //+7.0000e-02
52+
std::vector<std::string> linknames ={"Franka_link2_resp","Franka_link3_resp","Franka_link4_resp",
53+
"Franka_link5_resp","Franka_link6_resp","Franka_link7_resp",
54+
"Franka_link8_resp"};
55+
std::vector<std::string> jointnames ={"Franka_joint1", "Franka_joint2","Franka_joint3",
56+
"Franka_joint4", "Franka_joint5", "Franka_joint6",
57+
"Franka_joint7"};
58+
59+
60+
//------------------- Robot definition--------------------------
61+
//---------- Franka Emika Panda serial manipulator
62+
Eigen::Matrix<double,5,7> franka_mdh(5,7);
63+
franka_mdh << 0, 0, 0, 0, 0, 0, 0,
64+
0.333, 0, 3.16e-1, 0, 3.84e-1, 0, 0,
65+
0, 0, 0, 8.25e-2, -8.25e-2, 0, 8.8e-2,
66+
0, -M_PI_2, M_PI_2, M_PI_2, -M_PI_2, M_PI_2, M_PI_2,
67+
0, 0, 0, 0, 0, 0, 0;
68+
DQ_SerialManipulatorMDH franka(franka_mdh);
69+
DQ robot_base = 1 + E_ * 0.5 * DQ(0, 0.0413, 0, 0);
70+
franka.set_base_frame(robot_base);
71+
franka.set_reference_frame(robot_base);
72+
DQ robot_effector = 1+E_*0.5*k_*1.07e-1;
73+
franka.set_effector(robot_effector);
74+
75+
//Update the base of the robot from CoppeliaSim
76+
DQ new_base_robot;
77+
for (int i=0; i<=10;i++)
78+
{
79+
new_base_robot = (franka.get_base_frame())*vi.get_object_pose("Franka")*(1+0.5*E_*(-0.07*k_));
80+
}
81+
82+
franka.set_base_frame(new_base_robot);
83+
franka.set_reference_frame(new_base_robot);
84+
85+
//--------------------------------------------------------------
86+
//------------------- Controller definition---------------------
87+
DQ_QPOASESSolver solver;
88+
DQ_ClassicQPController controller
89+
(std::static_pointer_cast<DQ_Kinematics>
90+
(std::make_shared<DQ_SerialManipulatorMDH>(franka)),
91+
std::static_pointer_cast<DQ_QuadraticProgrammingSolver>
92+
(std::make_shared<DQ_QPOASESSolver>(solver)));
93+
94+
controller.set_gain(0.5);
95+
controller.set_damping(0.1);
96+
controller.set_control_objective(DQ_robotics::Translation);
97+
controller.set_stability_threshold(0.001);
98+
//--------------------------------------------------------------
99+
const double vfi_gain = 0.5;
100+
const double safe_angle = 15*(pi/180);
101+
const double T = 0.005;
102+
const double w = 0.2;
103+
const double Alpha = 20*(pi/180);
104+
double t = 0;
105+
106+
107+
for (int i=0;i<iterations;i++)
108+
{
109+
t = i*T;
110+
111+
double phi_t = Alpha*sin(w*t);
112+
double phi_t_dot = Alpha*w*cos(w*t);
113+
DQ r_dyn = cos(phi_t/2) + i_*sin(phi_t/2); //
114+
DQ r_dyn_dot = (-sin(phi_t/2) + i_*cos(phi_t/2))*(phi_t_dot/2);
115+
116+
VectorXd q = vi.get_joint_positions(jointnames);
117+
DQ x = franka.fkm(q);
118+
MatrixXd J = franka.pose_jacobian(q);
119+
120+
// DQ workspace_pose is a unit dual quaternion that represent the position and orientation of a frame rigidly attached
121+
// to the dynamic workspace line.
122+
DQ workspace_line_pose = r_dyn + 0.5*E_*x.translation()*r_dyn;
123+
124+
//----Dynamic Workspace line-------------------------
125+
DQ workspace_attached_direction = k_;
126+
DQ workspace_line = r_dyn * workspace_attached_direction *(r_dyn.conj());
127+
VectorXd vec_workspace_line_dot = (haminus4(k_*r_dyn.conj())+ hamiplus4(r_dyn*k_)*C4())*vec4(r_dyn_dot);
128+
129+
//----Robot Workspace line-------------------------
130+
const DQ robot_attached_direction = -k_;
131+
DQ robot_line = (x.P())*(robot_attached_direction)*(x.P().conj());
132+
133+
//----line-to-line-angle-Jacobian-------------------------//////
134+
//MatrixXd Jl = DQ_Kinematics::line_jacobian(J, x, robot_attached_direction);
135+
MatrixXd Jphi = DQ_Kinematics::
136+
line_to_line_angle_jacobian(DQ_Kinematics::line_jacobian(J, x, robot_attached_direction),
137+
robot_attached_direction,
138+
workspace_line);
139+
140+
double residual_phi = DQ_Kinematics::line_to_line_angle_residual(robot_line,
141+
workspace_line,
142+
DQ(vec_workspace_line_dot));
143+
144+
double phi = DQ_Geometry::line_to_line_angle(robot_line, workspace_line);
145+
double f = 2-2*cos(phi);
146+
double fsafe = 2-2*cos(safe_angle);
147+
double ferror = f-fsafe;
148+
if (-1*ferror < 0)
149+
{
150+
std::cout<<"-----RLINE_TO_LINE_ANGLE Constraint violated!!!!!!!!-------------------------"<<std::endl;
151+
}
152+
153+
if (USE_RESIDUAL == false)
154+
{
155+
residual_phi = 0;
156+
}
157+
VectorXd b(1);
158+
b(0) = vfi_gain*ferror + residual_phi;
159+
160+
vi.set_object_pose("x", x);
161+
vi.set_object_pose("cone", workspace_line_pose);
162+
vi.set_joint_position("Revolute_joint_master", safe_angle);
163+
164+
DQ xdesired = x;
165+
vi.set_object_pose("xd", xdesired);
166+
controller.set_inequality_constraint(Jphi, -b);
167+
VectorXd u = controller.compute_setpoint_control_signal(q, vec4(xdesired.translation()));
168+
std::cout << "Ending simulation at : " <<iterations-i<<std::endl;
169+
vi.set_joint_target_velocities(jointnames, u);
170+
vi.trigger_next_simulation_step();
171+
172+
}
173+
std::cout << "Stopping V-REP simulation..." << std::endl;
174+
vi.stop_simulation();
175+
vi.disconnect();
176+
return 0;
177+
}
178+

‎cmake/vrep_interface_tests/line_to_line_angle_jacobian/test_dynamic_conic_constraint.ttt‎

702 KB
Binary file not shown.

0 commit comments

Comments
 (0)