Commit 6deb98a6 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Add method to update weights of rigid contact

parent 2e0c5491
......@@ -8,6 +8,10 @@ import os
import gepetto.corbaserver
import time
import commands
import sys
sys.path += [os.getcwd()+'/../exercizes']
import plot_utils as plut
import matplotlib.pyplot as plt
np.set_printoptions(precision=3, linewidth=200, suppress=True)
......@@ -116,8 +120,8 @@ com_acc_ref = matlib.empty((3, N_SIMULATION))*nan
com_acc_des = matlib.empty((3, N_SIMULATION))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
offset = robot.com(data) + np.matrix([0.0, 0.0, 0.0]).T
amp = np.matrix([0.0, 0.03, 0.05]).T
two_pi_f = 2*np.pi*np.matrix([0.0, 0.5, 0.7]).T
amp = np.matrix([0.0, 0.03, 0.0]).T
two_pi_f = 2*np.pi*np.matrix([0.0, 2.0, 0.7]).T
two_pi_f_amp = np.multiply(two_pi_f,amp)
two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)
......@@ -171,3 +175,36 @@ for i in range(0, N_SIMULATION):
time_spent = time.time() - time_start
if(time_spent < dt): time.sleep(dt-time_spent)
# PLOT STUFF
time = np.arange(0.0, N_SIMULATION*dt, dt)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, com_pos[i,:].A1, label='CoM '+str(i))
ax[i].plot(time, com_pos_ref[i,:].A1, 'r:', label='CoM Ref '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM [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, com_vel[i,:].A1, label='CoM Vel '+str(i))
ax[i].plot(time, com_vel_ref[i,:].A1, 'r:', label='CoM Vel Ref '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM 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, com_acc[i,:].A1, label='CoM Acc '+str(i))
ax[i].plot(time, com_acc_ref[i,:].A1, 'r:', label='CoM Acc Ref '+str(i))
ax[i].plot(time, com_acc_des[i,:].A1, 'g--', label='CoM Acc Des '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM Acc [m/s^2]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
plt.show()
......@@ -116,6 +116,10 @@ namespace tsid
DEPRECATED bool addRigidContact(ContactBase & contact);
bool updateRigidContactWeights(const std::string & contact_name,
double force_regularization_weight,
double motion_weight=-1.0);
bool removeTask(const std::string & taskName,
double transition_duration=0.0);
......
......@@ -96,6 +96,17 @@ namespace tsid
DEPRECATED virtual bool addRigidContact(ContactBase & contact);
/**
* @brief Update the weights associated to the specified contact
* @param contact_name Name of the contact to update
* @param force_regularization_weight Weight of the force regularization task, if negative it is not updated
* @param motion_weight Weight of the motion task, if negative it is not update
* @return True if everything went fine, false otherwise
*/
virtual bool updateRigidContactWeights(const std::string & contact_name,
double force_regularization_weight,
double motion_weight=-1.0) = 0;
virtual bool removeTask(const std::string & taskName,
double transition_duration=0.0) = 0;
......
......@@ -237,15 +237,15 @@ bool InverseDynamicsFormulationAccForce::addRigidContact(ContactBase & contact,
resizeHqpData();
const ConstraintBase & motionConstr = contact.getMotionConstraint();
cl->motionConstraint = new ConstraintEquality(contact.name(), motionConstr.rows(), m_v+m_k);
cl->motionConstraint = new ConstraintEquality(contact.name()+"_motion_task", motionConstr.rows(), m_v+m_k);
m_hqpData[motionPriorityLevel].push_back(make_pair<double, ConstraintBase*>(motion_weight, cl->motionConstraint));
const ConstraintInequality & forceConstr = contact.getForceConstraint();
cl->forceConstraint = new ConstraintInequality(contact.name(), forceConstr.rows(), m_v+m_k);
cl->forceConstraint = new ConstraintInequality(contact.name()+"_force_constraint", forceConstr.rows(), m_v+m_k);
m_hqpData[0].push_back(make_pair<double, ConstraintBase*>(1.0, cl->forceConstraint));
const ConstraintEquality & forceRegConstr = contact.getForceRegularizationTask();
cl->forceRegTask = new ConstraintEquality(contact.name(), forceRegConstr.rows(), m_v+m_k);
cl->forceRegTask = new ConstraintEquality(contact.name()+"_force_reg_task", forceRegConstr.rows(), m_v+m_k);
m_hqpData[1].push_back(make_pair<double, ConstraintBase*>(force_regularization_weight, cl->forceRegTask));
m_eq += motionConstr.rows();
......@@ -260,6 +260,38 @@ bool InverseDynamicsFormulationAccForce::addRigidContact(ContactBase & contact)
return addRigidContact(contact, 1e-5);
}
bool InverseDynamicsFormulationAccForce::updateRigidContactWeights(const std::string & contact_name,
double force_regularization_weight,
double motion_weight)
{
// update weight of force regularization task
ConstraintLevel::iterator itt;
bool force_reg_task_found = false;
bool motion_task_found = false;
for(unsigned int i=1; i<m_hqpData.size(); i++)
{
for(itt=m_hqpData[i].begin(); itt!=m_hqpData[i].end(); itt++)
{
if(itt->second->name() == contact_name+"_force_reg_task")
{
if(force_regularization_weight>=0.0)
itt->first = force_regularization_weight;
if(motion_task_found)
return true;
force_reg_task_found = true;
}
else if(itt->second->name() == contact_name+"_motion_task")
{
if(motion_weight>=0.0)
itt->first = motion_weight;
if(force_reg_task_found)
return true;
motion_task_found = true;
}
}
}
}
const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double time,
ConstRefVector q,
......@@ -312,20 +344,6 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
const ConstraintEquality & fr = cl->contact.computeForceRegularizationTask(time, q, v, m_data);
cl->forceRegTask->matrix().middleCols(m_v+cl->index, m) = fr.matrix();
cl->forceRegTask->vector() = fr.vector();
// update weight of force regularization task
// ConstraintLevel::iterator itt;
// for(unsigned int i=1; i<m_hqpData.size(); i++)
// {
// for(itt=m_hqpData[i].begin(); itt!=m_hqpData[i].end(); itt++)
// {
// if(itt->second->name() == cl->contact.name())
// {
// itt->first = cl->contact.getForceRegularizationWeight();
// break;
// }
// }
// }
}
const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-6);
......@@ -400,6 +418,8 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
return m_hqpData;
}
bool InverseDynamicsFormulationAccForce::decodeSolution(const HQPOutput & sol)
{
if(m_solutionDecoded)
......@@ -536,13 +556,13 @@ bool InverseDynamicsFormulationAccForce::removeRigidContact(const std::string &
return false;
}
bool first_constraint_found = removeFromHqpData(contactName);
bool first_constraint_found = removeFromHqpData(contactName+"_motion_task");
assert(first_constraint_found);
bool second_constraint_found = removeFromHqpData(contactName);
bool second_constraint_found = removeFromHqpData(contactName+"_force_constraint");
assert(second_constraint_found);
bool third_constraint_found = removeFromHqpData(contactName);
bool third_constraint_found = removeFromHqpData(contactName+"_force_reg_task");
assert(third_constraint_found);
bool contact_found = false;
......
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