Skip to content
GitLab
Commit 9ee02ea9 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Add python bindings for method addActuationTask of... 

Add python bindings for method addActuationTask of inverse-dynamics-formulation-acc-force. Change name of method addTorqueTask to addActuationTask. Extend python example with UR5 robot with actuation bounds.
parent c5d08cee
Hide whitespace changes
Inline Side-by-side
bindings/python/formulations/formulation.hpp
View file @ 9ee02ea9
......@@ -30,6 +30,8 @@
#include "tsid/tasks/task-joint-posture.hpp"
#include "tsid/tasks/task-se3-equality.hpp"
#include "tsid/tasks/task-com-equality.hpp"
#include "tsid/tasks/task-actuation-bounds.hpp"
namespace tsid
{
namespace python
......@@ -54,7 +56,7 @@ namespace tsid
.def("addMotionTask", &InvDynPythonVisitor::addMotionTask_SE3, bp::args("task", "weight", "priorityLevel", "transition duration"))
.def("addMotionTask", &InvDynPythonVisitor::addMotionTask_COM, bp::args("task", "weight", "priorityLevel", "transition duration"))
.def("addMotionTask", &InvDynPythonVisitor::addMotionTask_Joint, bp::args("task", "weight", "priorityLevel", "transition duration"))
.def("addActuationTask", &InvDynPythonVisitor::addActuationTask_Bounds, bp::args("task", "weight", "priorityLevel", "transition duration"))
.def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight, bp::args("task_name", "weight"))
.def("addRigidContact", &InvDynPythonVisitor::addRigidContact6dDeprecated, bp::args("contact"))
......@@ -86,6 +88,9 @@ namespace tsid
static bool addMotionTask_Joint(T & self, tasks::TaskJointPosture & task, double weight, unsigned int priorityLevel, double transition_duration){
return self.addMotionTask(task, weight, priorityLevel, transition_duration);
}
static bool addActuationTask_Bounds(T & self, tasks::TaskActuationBounds & task, double weight, unsigned int priorityLevel, double transition_duration){
return self.addActuationTask(task, weight, priorityLevel, transition_duration);
}
static bool updateTaskWeight(T& self, const std::string & task_name, double weight){
return self.updateTaskWeight(task_name, weight);
}
......
exercizes/ex_1_ur5.py
View file @ 9ee02ea9
......@@ -5,20 +5,27 @@ from numpy.linalg import norm as norm
import matplotlib.pyplot as plt
import plot_utils as plut
import time
import ur5_conf as conf
from tsid_manipulator import TsidManipulator
import ur5_conf as conf
#import ur5_reaching_conf as conf
print "".center(conf.LINE_WIDTH,'#')
print " Task Space Inverse Dynamics - Manipulator ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'
PLOT_EE_POS = 1
PLOT_EE_VEL = 0
PLOT_EE_ACC = 0
PLOT_TORQUES = 1
tsid = TsidManipulator(conf)
N = conf.N_SIMULATION
tau = matlib.empty((tsid.robot.na, N))*nan
ee_pos = matlib.empty((3, N))*nan
ee_vel = matlib.empty((3, N))*nan
ee_acc = matlib.empty((3, N))*nan
ee_pos_ref = matlib.empty((3, N))*nan
ee_vel_ref = matlib.empty((3, N))*nan
ee_acc_ref = matlib.empty((3, N))*nan
......@@ -27,13 +34,10 @@ ee_acc_des = matlib.empty((3, N))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_
sampleEE = tsid.trajEE.computeNext()
samplePosture = tsid.trajPosture.computeNext()
amp = np.matrix([0.02, 0.1, 0.10]).T # amplitude
phi = np.matrix([0.0, 0.5*np.pi, 0.0]).T # phase
two_pi_f = 2*np.pi*np.matrix([1.0, 0.5, 0.5]).T # frequency (time 2 PI)
offset = sampleEE.pos()
two_pi_f_amp = np.multiply(two_pi_f,amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
offset[:3,0] += conf.offset
two_pi_f_amp = np.multiply(conf.two_pi_f, conf.amp)
two_pi_f_squared_amp = np.multiply(conf.two_pi_f, two_pi_f_amp)
pEE = offset.copy()
vEE = matlib.zeros((6,1))
......@@ -48,9 +52,9 @@ q, v = tsid.q, tsid.v
for i in range(0, N):
time_start = time.time()
pEE[:3,0] = offset[:3,0] + np.multiply(amp, matlib.sin(two_pi_f*t + phi))
vEE[:3,0] = np.multiply(two_pi_f_amp, matlib.cos(two_pi_f*t + phi))
aEE[:3,0] = np.multiply(two_pi_f_squared_amp, -matlib.sin(two_pi_f*t + phi))
pEE[:3,0] = offset[:3,0] + np.multiply(conf.amp, matlib.sin(conf.two_pi_f*t + conf.phi))
vEE[:3,0] = np.multiply(two_pi_f_amp, matlib.cos(conf.two_pi_f*t + conf.phi))
aEE[:3,0] = np.multiply(two_pi_f_squared_amp, -matlib.sin(conf.two_pi_f*t + conf.phi))
sampleEE.pos(pEE)
sampleEE.vel(vEE)
sampleEE.acc(aEE)
......@@ -61,10 +65,10 @@ for i in range(0, N):
sol = tsid.solver.solve(HQPData)
if(sol.status!=0):
print "QP problem could not be solved! Error code:", sol.status
print "Time %.3f QP problem could not be solved! Error code:"%t, sol.status
break
tau = tsid.formulation.getActuatorForces(sol)
tau[:,i] = tsid.formulation.getActuatorForces(sol)
dv = tsid.formulation.getAccelerations(sol)
ee_pos[:,i] = tsid.robot.framePosition(tsid.formulation.data(), tsid.EE).translation
......@@ -93,32 +97,47 @@ for i in range(0, N):
# PLOT STUFF
time = np.arange(0.0, N*conf.dt, conf.dt)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, ee_pos[i,:].A1, label='EE '+str(i))
ax[i].plot(time, ee_pos_ref[i,:].A1, 'r:', label='EE Ref '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('EE [m]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, ee_vel[i,:].A1, label='EE Vel '+str(i))
ax[i].plot(time, ee_vel_ref[i,:].A1, 'r:', label='EE Vel Ref '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('EE Vel [m/s]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, ee_acc[i,:].A1, label='EE Acc '+str(i))
ax[i].plot(time, ee_acc_ref[i,:].A1, 'r:', label='EE Acc Ref '+str(i))
ax[i].plot(time, ee_acc_des[i,:].A1, 'g--', label='EE Acc Des '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('EE Acc [m/s^2]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if(PLOT_EE_POS):
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, ee_pos[i,:].A1, label='EE '+str(i))
ax[i].plot(time, ee_pos_ref[i,:].A1, 'r:', label='EE Ref '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('EE [m]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if(PLOT_EE_VEL):
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, ee_vel[i,:].A1, label='EE Vel '+str(i))
ax[i].plot(time, ee_vel_ref[i,:].A1, 'r:', label='EE Vel Ref '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('EE Vel [m/s]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if(PLOT_EE_ACC):
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, ee_acc[i,:].A1, label='EE Acc '+str(i))
ax[i].plot(time, ee_acc_ref[i,:].A1, 'r:', label='EE Acc Ref '+str(i))
ax[i].plot(time, ee_acc_des[i,:].A1, 'g--', label='EE Acc Des '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('EE Acc [m/s^2]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if(PLOT_TORQUES):
(f, ax) = plut.create_empty_figure(tsid.robot.nv/2,2)
ax = ax.reshape(tsid.robot.nv)
for i in range(tsid.robot.nv):
ax[i].plot(time, tau[i,:].A1, label='Torque '+str(i))
ax[i].plot([time[0], time[-1]], 2*[tsid.tau_min[i,0]], ':')
ax[i].plot([time[0], time[-1]], 2*[tsid.tau_max[i,0]], ':')
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Torque [Nm]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
plt.show()
exercizes/tsid_manipulator.py
View file @ 9ee02ea9
......@@ -22,16 +22,16 @@ class TsidManipulator:
vector.extend(item for item in conf.path)
self.robot = tsid.RobotWrapper(conf.urdf, vector, False)
robot = self.robot
self.model = robot.model()
self.model = model = robot.model()
try:
q = se3.getNeutralConfiguration(robot.model(), conf.srdf, False)
# q = robot.model().referenceConfigurations["half_sitting"]
q = se3.getNeutralConfiguration(model, conf.srdf, False)
# q = model.referenceConfigurations["half_sitting"]
except:
q = conf.q0
# q = np.matrix(np.zeros(robot.nv)).T
v = np.matrix(np.zeros(robot.nv)).T
assert self.model.existFrame(conf.ee_frame_name)
assert model.existFrame(conf.ee_frame_name)
formulation = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
formulation.computeProblemData(0.0, q, v)
......@@ -46,10 +46,17 @@ class TsidManipulator:
self.eeTask.setKd(2.0 * np.sqrt(self.conf.kp_ee) * np.matrix(np.ones(6)).T)
self.eeTask.setMask(conf.ee_task_mask)
self.eeTask.useLocalFrame(False)
self.EE = robot.model().getFrameId(conf.ee_frame_name)
self.EE = model.getFrameId(conf.ee_frame_name)
H_ee_ref = self.robot.framePosition(formulation.data(), self.EE)
self.trajEE = tsid.TrajectorySE3Constant("traj-ee", H_ee_ref)
formulation.addMotionTask(self.eeTask, conf.w_ee, 1, 0.0)
self.tau_max = conf.tau_max_scaling*model.effortLimit
self.tau_min = -self.tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
if(conf.w_torque_bounds>0.0):
formulation.addActuationTask(actuationBoundsTask, conf.w_torque_bounds, 0, 0.0)
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q)
postureTask.setReference(trajPosture.computeNext())
......@@ -59,6 +66,7 @@ class TsidManipulator:
self.trajPosture = trajPosture
self.postureTask = postureTask
self.actuationBoundsTask = actuationBoundsTask
self.formulation = formulation
self.solver = solver
self.q = q
......
exercizes/ur5_conf.py
View file @ 9ee02ea9
......@@ -11,19 +11,28 @@ import os
np.set_printoptions(precision=3, linewidth=200, suppress=True)
LINE_WIDTH = 60
N_SIMULATION = 4000 # number of time steps simulated
N_SIMULATION = 10000 # number of time steps simulated
dt = 0.002 # controller time step
q0 = np.matrix([[ 0. , -1.0, 0.7, 0. , 0. , 0. ]]).T # initial configuration
ee_frame_name = "ee_fixed_joint" # end-effector frame name
ee_task_mask = np.matrix([1., 1, 1, 0, 0, 0]).T
# REFERENCE SINUSOIDAL TRAJECTORY
amp = np.matrix([0*0.02, 0.1, 0.10]).T # amplitude
phi = np.matrix([0.0, 0.5*np.pi, 0.0]).T # phase
two_pi_f = 1.4*2*np.pi*np.matrix([1.0, 0.5, 0.5]).T # frequency (time 2 PI)
offset = np.matrix([0.0, 0.0, 0.0]).T
w_ee = 1.0 # weight of end-effector task
w_ee = 1.0 # weight of end-effector task
w_posture = 1e-3 # weight of joint posture task
w_torque_bounds = 1.0 # weight of the torque bounds
kp_ee = 2.0 # proportional gain of end-effector constraint
kp_ee = 5.0 # proportional gain of end-effector constraint
kp_posture = 1.0 # proportional gain of joint posture task
tau_max_scaling = 0.4 # scaling factor of torque bounds
ee_frame_name = "ee_fixed_joint" # end-effector frame name
ee_task_mask = np.matrix([1., 1, 1, 0, 0, 0]).T
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps
CAMERA_TRANSFORM = [2.582354784011841, 1.620774507522583, 1.0674564838409424, 0.2770655155181885, 0.5401807427406311, 0.6969326734542847, 0.3817386031150818]
......
exercizes/ur5_reaching_conf.py 0 → 100644
View file @ 9ee02ea9
# -*- coding: utf-8 -*-
"""
Created on Thu Apr 18 09:47:07 2019
@author: student
"""
import numpy as np
import os
np.set_printoptions(precision=3, linewidth=200, suppress=True)
LINE_WIDTH = 60
N_SIMULATION = 3000 # number of time steps simulated
dt = 0.002 # controller time step
q0 = np.matrix([[ 0. , -1.0, 0.7, 0. , 0. , 0. ]]).T # initial configuration
# REFERENCE SINUSOIDAL TRAJECTORY
amp = np.matrix([0.0, 0.0, 0.0]).T # amplitude
phi = np.matrix([0.0, 0.5*np.pi, 0.0]).T # phase
two_pi_f = 2*np.pi*np.matrix([1.0, 0.5, 0.5]).T # frequency (time 2 PI)
offset = np.matrix([0.2, 0.0, 0.0]).T
w_ee = 1.0 # weight of end-effector task
w_posture = 1e-3 # weight of joint posture task
w_torque_bounds = 0.0 # weight of the torque bounds
kp_ee = 100.0 # proportional gain of end-effector constraint
kp_posture = 1.0 # proportional gain of joint posture task
tau_max_scaling = 0.4 # scaling factor of torque bounds
ee_frame_name = "ee_fixed_joint" # end-effector frame name
ee_task_mask = np.matrix([1., 1, 1, 0, 0, 0]).T
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps
CAMERA_TRANSFORM = [2.582354784011841, 1.620774507522583, 1.0674564838409424, 0.2770655155181885, 0.5401807427406311, 0.6969326734542847, 0.3817386031150818]
SPHERE_RADIUS = 0.03
REF_SPHERE_RADIUS = 0.03
EE_SPHERE_COLOR = (1, 0.5, 0, 0.5)
EE_REF_SPHERE_COLOR = (1, 0, 0, 0.5)
ERROR_MSG = 'You should set the environment variable UR5_MODEL_DIR to something like "$DEVEL_DIR/install/share"\n';
path = os.environ.get('UR5_MODEL_DIR', ERROR_MSG)
urdf = path + "/ur_description/urdf/ur5_robot.urdf";
srdf = path + '/ur_description/srdf/ur5_robot.srdf'
\ No newline at end of file
include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp
View file @ 9ee02ea9
......@@ -103,10 +103,10 @@ namespace tsid
unsigned int priorityLevel,
double transition_duration=0.0);
bool addTorqueTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0);
bool addActuationTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0);
bool updateTaskWeight(const std::string & task_name,
double weight);
......
include/tsid/formulations/inverse-dynamics-formulation-base.hpp
View file @ 9ee02ea9
......@@ -74,10 +74,10 @@ namespace tsid
unsigned int priorityLevel,
double transition_duration=0.0) = 0;
virtual bool addTorqueTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0) = 0;
virtual bool addActuationTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration=0.0) = 0;
virtual bool updateTaskWeight(const std::string & task_name,
double weight) = 0;
......
src/formulations/inverse-dynamics-formulation-acc-force.cpp
View file @ 9ee02ea9
......@@ -166,10 +166,10 @@ bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
}
bool InverseDynamicsFormulationAccForce::addTorqueTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration)
bool InverseDynamicsFormulationAccForce::addActuationTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double transition_duration)
{
assert(weight>=0.0);
assert(transition_duration>=0.0);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment