Commit a162facf authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

Merge branch 'endEnffectorTest' into 'devel'

End effector test

See merge request !6
parents 85352b08 39c2203f
......@@ -181,6 +181,8 @@ IF(BUILD_PYTHON_INTERFACE)
python/${SOTTALOSBALANCE_PYNAME}/test/test_comAdmittance.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_admittance_single_joint.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_end_effector.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_admittance_end_effector.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint_velocity_based.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_admittance_single_joint_velocity_based.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_singleTraj.py
......
......@@ -15,8 +15,8 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (admittance_controller_end_effector_EXPORTS)
#if defined(WIN32)
# if defined(admittance_controller_end_effector_EXPORTS)
# define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT __declspec(dllexport)
# else
# define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT __declspec(dllimport)
......@@ -25,7 +25,6 @@
# define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......@@ -37,97 +36,123 @@
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/multibody/model.hpp>
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/motion.hpp"
#include <sot/core/robot-utils.hh>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>
namespace dynamicgraph
{
namespace sot
{
namespace talos_balance
{
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/**
* @brief Admittance controller for an upper body end-effector (right or left wrist
*
* This entity computes a velocity reference for an end-effector based on the force error in the world frame :
* dqRef = integral( Kp(forceDes-forceWorldFrame) )
*
*/
class ADMITTANCECONTROLLERENDEFFECTOR_EXPORT AdmittanceControllerEndEffector
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceControllerEndEffector(const std::string & name);
/* --- SIGNALS --- */
/// \brief Gain (6d) for the integration of the error on the force
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
/// \brief 6d force given by the sensor in its local frame
DECLARE_SIGNAL_IN(force, dynamicgraph::Vector);
/// \brief 6d desired force of the end-effector in the world frame
DECLARE_SIGNAL_IN(forceDes, dynamicgraph::Vector);
/// \brief Current joint configuration of the robot
DECLARE_SIGNAL_IN(jointPosition, dynamicgraph::Vector);
/// \brief Velocity reference for the end-effector computed by the controller
DECLARE_SIGNAL_OUT(dqRef, dynamicgraph::Vector);
/// \brief 6d force given by the sensor in he global frame
DECLARE_SIGNAL_INNER(forceWorldFrame, dynamicgraph::Vector);
/* --- COMMANDS --- */
/**
* @brief Initialize the entity
*
* @param[in] dt Time step of the control
* @param[in] sensorFrameName Name of the force sensor of the end-effector used in the pinocchio model
*/
void init(const double & dt, const std::string & sensorFrameName);
/**
* @brief Reset the velocity
*/
void reset_dq();
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
protected:
/// Dimension of the force signals and of the output
int m_n;
/// True if the entity has been successfully initialized
bool m_initSucceeded;
/// Internal state
dynamicgraph::Vector m_dq;
/// Time step of the control
double m_dt;
/// Robot Util instance to get the sensor frame
RobotUtilShrPtr m_robot_util;
/// Pinocchio robot model
pinocchio::Model m_model;
/// Pinocchio robot data
pinocchio::Data *m_data;
/// Force sensor frame placement wrt the parent frame
pinocchio::SE3 m_sensorFrame;
/// Id of the parent joint of the force sensor frame
pinocchio::JointIndex m_parentId;
/// robot configuration according to pinocchio convention
dynamicgraph::Vector m_q;
}; // class AdmittanceControllerEndEffector
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/**
* @brief Admittance controller for an upper body end-effector (right or left
* wrist)
*
* This entity computes a velocity reference for an end-effector based
* on the force error in the world frame :
* w_dq = integral(Kp(w_forceDes-w_force)) + Kd (w_dq)
*
*/
class ADMITTANCECONTROLLERENDEFFECTOR_EXPORT AdmittanceControllerEndEffector
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceControllerEndEffector(const std::string &name);
/* --- SIGNALS --- */
/// \brief Gain (6d) for the integration of the error on the force
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
/// \brief Derivative gain (6d) for the error on the force
DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);
/// \brief Value of the saturation to apply on the velocity output
DECLARE_SIGNAL_IN(dqSaturation, dynamicgraph::Vector);
/// \brief 6d force given by the sensor in its local frame
DECLARE_SIGNAL_IN(force, dynamicgraph::Vector);
/// \brief 6d desired force of the end-effector in the world frame
DECLARE_SIGNAL_IN(w_forceDes, dynamicgraph::Vector);
/// \brief Current joint configuration of the robot
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
/// \brief 6d force given by the sensor in the world frame
DECLARE_SIGNAL_INNER(w_force, dynamicgraph::Vector);
/// \brief Internal intergration computed in the world frame
DECLARE_SIGNAL_INNER(w_dq, dynamicgraph::Vector);
/// \brief Velocity reference for the end-effector in the local frame
DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector);
/* --- COMMANDS --- */
/**
* @brief Initialize the entity
*
* @param[in] dt Time step of the control
* @param[in] sensorFrameName Name of the force sensor of the end-effector
* used in the pinocchio model
*/
void init(const double &dt, const std::string &sensorFrameName,
const std::string &endeffectorName, const bool &removeWeight);
/**
* @brief Reset the velocity
*/
void resetDq();
/**
* @brief Set to true if you want to remove the weight
*
* @param[in] removeWeight Boolean used to remove the weight or not
*/
void setRemoveWeight(const bool &removeWeight);
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
protected:
/// Dimension of the force signals and of the output
int m_n;
/// True if the entity has been successfully initialized
bool m_initSucceeded;
/// Internal state
dynamicgraph::Vector m_w_dq;
/// Time step of the control
double m_dt;
// Weight of the end-effector
double m_mass;
// If true, the weight of the end effector is removed from the force
bool m_removeWeight;
/// Robot Util instance to get the sensor frame
RobotUtilShrPtr m_robot_util;
/// Pinocchio robot model
pinocchio::Model m_model;
/// Pinocchio robot data
pinocchio::Data *m_data;
/// Id of the force sensor frame
pinocchio::FrameIndex m_sensorFrameId;
/// Id of the joint of the end-effector
pinocchio::JointIndex m_endEffectorId;
/// robot configuration according to pinocchio convention
dynamicgraph::Vector m_q;
const double END_EFFECTOR_WEIGHT = -14.604817920170488;
}; // class AdmittanceControllerEndEffector
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_admittance_controller_end_effector_H__
......@@ -6,4 +6,4 @@ Created on Mon Feb 9 13:55:16 2015
import numpy as np
NJ = 38
CTRL_MAX = 0.3 # max desired control (security check of ControlManager)
CTRL_MAX = 0.3 # max desired control (security check of ControlManager)
\ No newline at end of file
......@@ -6,7 +6,7 @@ from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep;
dt = robot.timeStep
robot.comTrajGen = create_com_trajectory_generator(dt,robot);
# --- COM
......
import sot_talos_balance.talos.parameter_server_conf as paramServerConfig
import sot_talos_balance.talos.control_manager_conf as controlManagerConfig
import sot_talos_balance.talos.base_estimator_conf as baseEstimatorConf
from sot_talos_balance.create_entities_utils import *
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
from dynamic_graph.sot.core import SOT, Task, GainAdaptive, FeaturePosture
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
from dynamic_graph.ros import RosSubscribe
from dynamic_graph.ros import RosPublish
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import eye
from dynamic_graph.ros import RosSubscribe, RosPublish
import numpy as np
import math
from dynamic_graph.tracer_real_time import TracerRealTime
from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
robot.timeStep = robot.device.getTimeStep()
timeStep = robot.timeStep
# --- CREATE PARAM_SERVER
robot.param_server = create_parameter_server(paramServerConfig, timeStep)
# --- SET INITIAL CONFIGURATION ------------------------------------------------
q = [0., 0., 1.018213, 0., 0., 0.] # Base
q += [0., 0., -0.411354, 0.859395, -0.448041, -0.001708] # Left Leg
q += [0., 0., -0.411354, 0.859395, -0.448041, -0.001708] # Right Leg
q += [0.0, 0.006761] # Chest
q += [0.25847, 0.173046, -0.0002, -0.525366, 0., 0., 0.1, -0.005] # Left Arm
# q += [-0.25847, -0.173046, 0.0002, -0.525366, 0., 0., 0.1, -0.005] # Right Arm
q += [-0.25847, -0.0, 0.19, -1.61, 0., 0., 0.1, -0.005] #Right Arm
q += [0., 0.] # Head
robot.device.set(q)
# --- CREATE ENTITIES ----------------------------------------------------------
robot.param_server = create_parameter_server(paramServerConfig, robot.timeStep)
robot.device_filters = create_device_filters(robot, robot.timeStep)
robot.imu_filters = create_imu_filters(robot, robot.timeStep)
robot.baseEstimator = create_base_estimator(robot, robot.timeStep, baseEstimatorConf)
robot.controller = create_end_effector_admittance_controller(robot, 'rightWrist')
robot.controlManager = create_ctrl_manager(controlManagerConfig, robot.timeStep)
robot.controlManager.addCtrlMode('sot_input')
robot.controlManager.setCtrlMode('all', 'sot_input')
# --- ADMITTANCE CONTROLLER ---
Kp = np.array([0.0, 0., 0., 0., 0., 0.])
robot.admittanceController = create_end_effector_admittance_controller(Kp, timeStep, robot)
robot.admittanceController.forceDes.value = [0., 0., 0., 0., 0., 0.]
# --- HAND TASK ----------------------------------------------------------------
# --- HAND TASK ---
# Create task
taskRightHand = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist'])
handMgrip = eye(4)
taskRightHand = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'arm_right_7_joint')
handMgrip = np.eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRightHand.opmodif = matrixToTuple(handMgrip)
taskRightHand.feature.frame('current')
taskRightHand.feature.selec.value = '000111'
taskRightHand.feature.frame('desired')
taskRightHand.feature.selec.value = '111111'
taskRightHand.task.setWithDerivative(True)
taskRightHand.task.controlGain.value = 0
taskRightHand.feature.position.value = eye(4)
taskRightHand.feature.velocity.value = [0., 0., 0., 0., 0., 0.]
taskRightHand.featureDes.position.value = eye(4)
plug(robot.admittanceController.dqRef, taskRightHand.featureDes.velocity)
## --- STATIC COM (if not walking)
#taskCom = MetaTaskKineCom(robot.dynamic)
#robot.dynamic.com.recompute(0)
#taskCom.featureDes.errorIN.value = robot.dynamic.com.value
#taskCom.task.controlGain.value = 10
# --- CONTACTS
# Define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(100)
robot.contactLF.keep()
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(100)
robot.contactRF.keep()
locals()['contactRF'] = robot.contactRF
taskRightHand.feature.position.value = np.eye(4)
taskRightHand.feature.velocity.value = [0., 0., 0., 0., 0., 0.]
taskRightHand.featureDes.position.value = np.eye(4)
plug(robot.controller.dq, taskRightHand.featureDes.velocity)
# --- BASE TASK ----------------------------------------------------------------
taskWaist = MetaTaskKine6d('taskWaist', robot.dynamic, 'WT', robot.OperationalPointsMap['waist'])
taskWaist.feature.frame('desired')
taskWaist.gain.setConstant(300)
taskWaist.keep()
taskWaist.feature.selec.value = '111111'
locals()['taskWaist'] = taskWaist
# --- POSTURE TASK -------------------------------------------------------------
robot.taskPosture = Task('task_posture')
robot.taskPosture.controlGain.value = 100.0
robot.taskPosture.feature = FeaturePosture('feature_posture')
q = list(robot.dynamic.position.value)
robot.taskPosture.feature.state.value = q
robot.taskPosture.feature.posture.value = q
robot.taskPosture.feature.selectDof(6, True)
robot.taskPosture.feature.selectDof(7, True)
robot.taskPosture.feature.selectDof(8, True)
robot.taskPosture.feature.selectDof(9, True)
robot.taskPosture.feature.selectDof(10, True)
robot.taskPosture.feature.selectDof(11, True)
robot.taskPosture.feature.selectDof(12, True)
robot.taskPosture.feature.selectDof(13, True)
robot.taskPosture.feature.selectDof(14, True)
robot.taskPosture.feature.selectDof(15, True)
robot.taskPosture.feature.selectDof(16, True)
robot.taskPosture.feature.selectDof(17, True)
robot.taskPosture.feature.selectDof(18, True)
robot.taskPosture.feature.selectDof(19, True)
robot.taskPosture.add(robot.taskPosture.feature.name)
plug(robot.dynamic.position, robot.taskPosture.feature.state)
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control, robot.device.control)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
#robot.sot.push(taskCom.task.name)
robot.device.control.recompute(0)
# Plug SOT control to device through control manager
plug(robot.sot.control, robot.controlManager.ctrl_sot_input)
plug(robot.controlManager.u_safe, robot.device.control)
# plug(robot.sot.control, robot.device.control)
# --- PUSH THE TASKS -----------------------------------------------------------
robot.sot.push(robot.taskPosture.name)
robot.sot.push(taskRightHand.task.name)
robot.sot.push(taskWaist.task.name)
# # --- ROS PUBLISHER ----------------------------------------------------------
## --- ROS PUBLISHER
robot.publisher = create_rospublish(robot, 'robot_publisher')
create_topic(robot.publisher, robot.admittanceController, 'forceWorldFrame', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.admittanceController, 'force', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.admittanceController, 'dqRef', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.admittanceController, 'forceDes', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.controller, 'w_force', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.controller, 'force', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.controller, 'dq', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.controller, 'w_dq', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.controller, 'w_forceDes', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.device, 'velocity', robot=robot, data_type='vector')
# --- ROS SUBSCRIBER
# # --- ROS SUBSCRIBER
robot.subscriber = RosSubscribe("end_effector_subscriber")
robot.subscriber.add("vector","forceWorldFrame","/sot/admittanceController/forceWorldFrame")
robot.subscriber.add("vector","force","/sot/admittanceController/force")
robot.subscriber.add("vector","dqRef","/sot/admittanceController/dqRef")
robot.subscriber.add("vector","forceDes","/sot/admittanceController/forceDes")
robot.subscriber.add("vector", "w_force", "/sot/controller/w_force")
robot.subscriber.add("vector", "force", "/sot/controller/force")
robot.subscriber.add("vector", "dq", "/sot/controller/dq")
robot.subscriber.add("vector", "w_dq", "/sot/controller/w_dq")
robot.subscriber.add("vector", "w_forceDes", "/sot/controller/w_forceDes")
robot.subscriber.add("vector", "velocity", "/sot/device/velocity")
# --- TRACER ------------------------------------------------------------------
robot.tracer = TracerRealTime("force_tracer")
robot.tracer.setBufferSize(80*(2**20))
robot.tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
addTrace(robot.tracer, robot.controller, 'force')
addTrace(robot.tracer, robot.controller, 'w_force')
addTrace(robot.tracer, robot.controller, 'w_dq')
addTrace(robot.tracer, robot.controller, 'dq')
robot.tracer.start()
......@@ -8,7 +8,7 @@ N_JOINTS = 32
N_CONFIG = N_JOINTS + 6
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep;
dt = robot.timeStep
JOINT = 25
QJOINT = JOINT + 6
......
from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
from time import sleep
from sot_talos_balance.utils.gazebo_utils import apply_force
import matplotlib.pyplot as plt
import numpy as np
run_test('appli_admittance_end_effector.py')
sleep(1.0)
sleep(10.0)
# --- DISPLAY
# force_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.controller.name') + '-force.dat')
# w_force_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.controller.name') + '-w_force.dat')
runCommandClient('robot.sot.push(taskRightHand.task.name)')
# plt.ion()
sleep(50.0)
# plt.figure()
# plt.plot(force_data[:,1],'b-')
# plt.plot(force_data[:,2],'r-')
# plt.plot(force_data[:,3],'g-')
# plt.title('force')
# plt.legend(['force x', 'force y', 'force z'])
forceWorldFrame = evalCommandClient('robot.admittanceController.forceWorldFrame.value')
# plt.figure()
# plt.plot(w_force_data[:,1],'b-')
# plt.plot(w_force_data[:,2],'r-')
# plt.plot(w_force_data[:,3],'g-')
# plt.title('w_force')
# plt.legend(['w_force x', 'w_force y', 'w_force z'])
print("Current force: ")
print(forceWorldFrame)
raw_input("Wait before leaving the simulation")
runCommandClient('robot.admittanceController.forceDes.value = robot.admittanceController.forceWorldFrame.value')
# runCommandClient('robot.admittanceController.Kp.value = (0.00001, 0.00001, 0.00001, 0.0, 0.0, 0.0)')
runCommandClient('dump_tracer(robot.tracer)')
\ No newline at end of file
......@@ -10,54 +10,59 @@ import numpy as np
from time import sleep
import os
class Bunch:
def __init__(self, **kwds):
self.__dict__.update(kwds)
def __str__(self, prefix=""):
res = ""
for (key,value) in self.__dict__.iteritems():
if (isinstance(value, np.ndarray) and len(value.shape)==2 and value.shape[0]>value.shape[1]):
res += prefix+" - " + key + ": " + str(value.T) + "\n"
elif (isinstance(value, Bunch)):
res += prefix+" - " + key + ":\n" + value.__str__(prefix+" ") + "\n"
else:
res += prefix+" - " + key + ": " + str(value) + "\n"
return res[:-1]
def __init__(self, **kwds):
self.__dict__.update(kwds)
def __str__(self, prefix=""):
res = ""
for (key, value) in self.__dict__.iteritems():
if (isinstance(value, np.ndarray) and len(value.shape) == 2 and value.shape[0] > value.shape[1]):
res += prefix+" - " + key + ": " + str(value.T) + "\n"
elif (isinstance(value, Bunch)):
res += prefix+" - " + key + ":\n" + value.__str__(prefix+" ") + "\n"
else:
res += prefix+" - " + key + ": " + str(value) + "\n"
return res[:-1]
def start_sot():
os.system('rosservice call /start_dynamic_graph')
os.system('rosservice call /start_dynamic_graph')
def stop_sot():
os.system('rosservice call /stop_dynamic_graph')
os.system('rosservice call /stop_dynamic_graph')
def smoothly_set_signal_to_zero(sig):
v = np.array(sig.value)
for i in range(40):
v = 0.95*v
sig.value = tuple(v)
sleep(1)
print('Setting signal to zero')
v[:] = 0.0
v = np.array(sig.value)
for i in range(40):
v = 0.95*v
sig.value = tuple(v)
def smoothly_set_signal(sig, final_value, duration=5.0, steps=500, prints = 10):
v = np.array(sig.value)
vf = np.array(final_value)
for i in range(steps+1):
alpha = 1.0*i/steps
sig.value = tuple(vf*alpha+(1-alpha)*v)
sleep(1.0*duration/steps)
print('Signal set')
sig.value = tuple(final_value)