Skip to content

Commit e63c149

Browse files
fbrand-newfbrand-new
committed
Added r1 cartesian control besides ergocub cart control. R1 cart control is currently the old version of grasping baselines because the new version does not currently work
1 parent 6b6601b commit e63c149

16 files changed

+2237
-0
lines changed

‎CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ add_subdirectory(src/ergocub_cartesian_service)
2828

2929
# Add module
3030
add_subdirectory(src/ergocub_cartesian_control)
31+
add_subdirectory(src/r1_cartesian_control)
3132

3233

3334
# Add standard uninstall target

‎src/r1_cartesian_control/CMakeLists.txt

+60
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
set(EXE_TARGET_NAME r1-cartesian-control)
2+
3+
# YARP
4+
find_package(YARP REQUIRED COMPONENTS
5+
dev
6+
os
7+
sig
8+
idl_tools
9+
eigen
10+
)
11+
12+
# Eigen3
13+
find_package(Eigen3 REQUIRED)
14+
15+
# iDynTree
16+
find_package(iDynTree REQUIRED)
17+
18+
# ProxSuite
19+
find_package(proxsuite REQUIRED)
20+
21+
# Executable
22+
add_executable(${EXE_TARGET_NAME}
23+
include/DifferentialInverseKinematics.h
24+
include/DifferentialInverseKinematicsQP.h
25+
include/ForwardKinematics.h
26+
include/ForwardKinematicsiDynTree.h
27+
include/Integrator.h
28+
include/module.h
29+
src/DifferentialInverseKinematicsQP.cpp
30+
src/ForwardKinematicsiDynTree.cpp
31+
src/Integrator.cpp
32+
src/main.cpp
33+
src/module.cpp
34+
src/module_service.cpp
35+
)
36+
37+
target_include_directories(${EXE_TARGET_NAME} PRIVATE
38+
${CMAKE_CURRENT_SOURCE_DIR}/include
39+
)
40+
41+
target_link_libraries(${EXE_TARGET_NAME} PRIVATE
42+
YARP::YARP_dev
43+
YARP::YARP_init
44+
YARP::YARP_os
45+
YARP::YARP_sig
46+
YARP::YARP_eigen
47+
Eigen3::Eigen
48+
${iDynTree_LIBRARIES}
49+
proxsuite::proxsuite
50+
gb-ergocub-cartesian-service
51+
cub-joint-control
52+
utils
53+
)
54+
55+
# Install the executable
56+
install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin)
57+
58+
# Install configuration and application files
59+
install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/app/conf/config_left_sim_r1.ini DESTINATION ${YARP_CONTEXTS_INSTALL_DIR}/${EXE_TARGET_NAME})
60+
install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/app/conf/config_right_sim_r1.ini DESTINATION ${YARP_CONTEXTS_INSTALL_DIR}/${EXE_TARGET_NAME})

‎src/r1_cartesian_control/app/conf/config_left_sim_r1.ini

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
rate 100.0
2+
traj_duration 5.0
3+
rpc_local_port_name /gb-ergocub-cartesian-controller/left_arm/rpc:i
4+
position_error_th 0.0005
5+
max_iteration 10000
6+
module_logging false
7+
module_verbose false
8+
qp_verbose false
9+
10+
[ARM]
11+
12+
name left_arm
13+
14+
joint_axes_list (torso_yaw_joint l_shoulder_pitch l_shoulder_roll l_shoulder_yaw l_elbow l_wrist_yaw l_wrist_roll l_wrist_pitch) #order matters
15+
joint_ports_list (/r1mk3Sim/torso /r1mk3Sim/left_arm)
16+
joint_local_port /gb-ergocub-cartesian-controller/left_arm
17+
18+
[IK_PARAM]
19+
20+
limits_param 0.90 # 0 < limits_param < 1
21+
joint_vel_weight (5.0 0.0) #(cost weight, cost offset)
22+
position_param (20.0 0.5) #(cost weight, proportional gain)
23+
orientation_param (10.0 0.5) #(cost weight, proportional gain)
24+
joint_pos_param (2.5 10.0 5.0) #(cost weight, proportional gain, derivative gain)
25+
max_joint_position_variation 25.0 #[deg]
26+
max_joint_position_track_error 1.5 #[deg]
27+
#joint_ref (0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0)
28+
29+
[FK_PARAM]
30+
31+
root_frame_name base_link
32+
ee_frame_name l_hand_palm
33+
34+
[FSM_PARAM]
35+
36+
stop_speed 0.001 #[rad] = 0.286478898 deg

‎src/r1_cartesian_control/app/conf/config_right_sim_r1.ini

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
rate 100.0
2+
traj_duration 5.0
3+
rpc_local_port_name /gb-ergocub-cartesian-controller/right_arm/rpc:i
4+
position_error_th 0.0005
5+
max_iteration 10000
6+
module_logging false
7+
module_verbose false
8+
qp_verbose false
9+
10+
[ARM]
11+
12+
name right_arm
13+
14+
joint_axes_list (torso_yaw_joint r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow r_wrist_yaw r_wrist_roll r_wrist_pitch) #order matters
15+
joint_ports_list (/r1mk3Sim/torso /r1mk3Sim/right_arm)
16+
joint_local_port /gb-ergocub-cartesian-controller/right_arm
17+
18+
[IK_PARAM]
19+
20+
limits_param 0.90 # 0 < limits_param < 1
21+
joint_vel_weight (5.0 0.0) #(cost weight, cost offset)
22+
position_param (20.0 0.5) #(cost weight, proportional gain)
23+
orientation_param (10.0 0.5) #(cost weight, proportional gain)
24+
joint_pos_param (2.5 10.0 5.0) #(cost weight, proportional gain, derivative gain)
25+
max_joint_position_variation 25.0 #[deg]
26+
max_joint_position_track_error 1.5 #[deg]
27+
#joint_ref (0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0)
28+
29+
[FK_PARAM]
30+
31+
root_frame_name base_link
32+
ee_frame_name r_hand_palm
33+
34+
[FSM_PARAM]
35+
36+
stop_speed 0.001 #[rad] = 0.286478898 deg

‎src/r1_cartesian_control/include/DifferentialInverseKinematics.h

+60
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
/*
2+
* Copyright (C) 2023 Istituto Italiano di Tecnologia (IIT)
3+
*
4+
* This software may be modified and distributed under the terms of the
5+
* MIT license. See the accompanying LICENSE file for details.
6+
*/
7+
8+
#ifndef DIFFERENTIAL_INVERSE_KINEMATICS_H
9+
#define DIFFERENTIAL_INVERSE_KINEMATICS_H
10+
11+
#include <Eigen/Dense>
12+
#include <optional>
13+
14+
/**
15+
* Abstract class representing the differential inverse kinematics of a kinematic chain.
16+
*/
17+
class DifferentialInverseKinematics
18+
{
19+
public:
20+
virtual ~DifferentialInverseKinematics() = default;
21+
22+
/**
23+
* Evaluate the reference joints velocities that drive the robot end-effector towards the desired transform.
24+
* @return The reference velocities that realize the inverse kinematics in radian/s.
25+
* @warning This method should be called only after setting the current robot state via DifferentialInverseKinematics::set_robot_state()
26+
* and the desired end-effector transform via DifferentialInverseKinematics::set_desired_ee_transform().
27+
*/
28+
virtual std::optional<Eigen::VectorXd> eval_reference_velocities() = 0;
29+
30+
/**
31+
* Set the current robot state.
32+
* @param joints The current joint configuration \f$ q \f$.
33+
* @param transform The current end-effector transformation.
34+
* @param jacobian The current end-effector Jacobian.
35+
*/
36+
virtual void set_robot_state(
37+
const Eigen::Ref<const Eigen::VectorXd> &joints,
38+
const Eigen::Ref<const Eigen::VectorXd> &joints_vel,
39+
const Eigen::Transform<double, 3, Eigen::Affine> &transform,
40+
const Eigen::Ref<const Eigen::MatrixXd> &jacobian) = 0;
41+
42+
/**
43+
* Set the desired root to end-effector transform.
44+
* @param transform The desired end-effector transformation.
45+
*/
46+
virtual void set_desired_ee_transform(const Eigen::Transform<double, 3, Eigen::Affine> &transform) = 0;
47+
48+
/**
49+
* Set the joints limits.
50+
* @param lower_limits The joint lower limits \f$q_{L}\f$.
51+
* @param upper_limits The joint upper limits \f$q_{U}\f$.
52+
* @param gains The joint limits gains \f$K_l\f$.
53+
*/
54+
virtual void set_joint_limits(
55+
const Eigen::Ref<const Eigen::VectorXd> &lower_limits,
56+
const Eigen::Ref<const Eigen::VectorXd> &upper_limits,
57+
const Eigen::Ref<const Eigen::VectorXd> &gains) = 0;
58+
};
59+
60+
#endif /* DIFFERENTIAL_INVERSE_KINEMATICS_H */

0 commit comments

Comments
 (0)