19 #include "tgCPGStringControl.h"
28 tgCPGStringControl::tgCPGStringControl(
const double controlStep) :
30 m_controlStep(controlStep),
31 m_commandedTension(0.0),
35 if (m_controlStep < 0.0)
37 throw std::invalid_argument(
"Negative control step");
41 tgCPGStringControl::~tgCPGStringControl()
51 m_pFromBody = subject.
getMuscle()->anchor1->attachedBody;
52 m_pToBody = subject.
getMuscle()->anchor2->attachedBody;
61 if (m_controlTime >= m_controlStep)
63 m_commandedTension = motorControl().
control(&subject, dt, controlLength(), getCPGValue());
71 assert(m_nodeNumber == -1);
73 m_pCPGSystem = &CPGSys;
75 std::vector<double> params (7);
76 params[0] = nodeParams[0][0];
77 params[1] = nodeParams[0][0];
78 params[2] = nodeParams[0][1];
79 params[3] = nodeParams[0][1];
84 m_nodeNumber = m_pCPGSystem->addNode(params);
91 assert(m_nodeNumber >= 0);
93 int muscleSize = edgeParams.shape()[1];
95 std::vector<int> connectivityList;
96 std::vector<double> weights;
97 std::vector<double> phases;
102 for (
int i = 0; i < allStrings.size(); i++)
104 if (
this != allStrings[i])
107 assert(m_pCPGSystem == allStrings[i]->
getCPGSys());
109 const btRigidBody* theirFromGroup = allStrings[i]->getFromBody();
110 const btRigidBody* theirToGroup = allStrings[i]->getToBody();
113 if(m_pFromBody == theirFromGroup ||
114 m_pToBody == theirToGroup ||
115 m_pFromBody == theirToGroup ||
116 m_pToBody == theirFromGroup)
118 int theirMuscle = allStrings[i]->getNodeNumber();
120 int rp = ((m_nodeNumber - theirMuscle) / muscleSize) + 1;
121 int j = m_nodeNumber % muscleSize;
122 int k = theirMuscle % muscleSize;
123 connectivityList.push_back(theirMuscle);
126 weights.push_back(edgeParams[rp][k][j][0]);
127 phases.push_back(edgeParams[rp][k][j][1]);
131 weights.push_back(edgeParams[rp][j][k][0]);
132 phases.push_back(edgeParams[rp][j][k][1]);
138 m_pCPGSystem->defineConnections(m_nodeNumber, connectivityList, weights, phases);
143 tgBaseCPGNode::setupControl(ipc);
147 double controlLength)
149 if (controlLength < 0.0)
151 throw std::invalid_argument(
"Negative control length");
154 m_controlLength = controlLength;
155 tgBaseCPGNode::setupControl(ipc);
const Muscle2P * getMuscle() const
virtual void onAttach(tgLinearString &subject)
virtual void onStep(tgLinearString &subject, double dt)
const CPGEquations * getCPGSys() const
Definitions of classes Muscle2P and MuscleAnchor.
double control(tgBaseString *const mString, double deltaTimeSeconds, double newPosition, double offsetVel=0)
Control Functions.
void setConnectivity(const std::vector< tgCPGStringControl * > &allStrings, array_4D edgeParams)
Contains the definition of class ImpedanceControl. $Id$.
void assignNodeNumber(CPGEquations &CPGSys, array_2D nodeParams)
Definition of class CPGEquations.
virtual const double getStartLength() const