26 #define DEG 57.295779524 27 #define MANUAL_AXES0_OFFSET 1.8 28 #define MANUAL_AXES6_OFFSET 1.5 30 #define SIM_CLOCK_FREQUENCY 10.0 // ms 33 #ifndef PCTRL_CHECK_INITIALIZED() 34 #define PCTRL_CHECK_INITIALIZED() \ 35 if (isInitialized() == false) \ 37 m_ErrorMessage.assign("Manipulator not initialized."); \ 46 m_SimThreadArgs = NULL;
61 m_DOF = params->GetNumberOfDOF();
64 m_NumOfModules = m_DOF;
72 m_CurrentAngles.resize(m_DOF);
73 m_CurrentAngularMaxAccel.resize(m_DOF);
74 m_CurrentAngularMaxVel.resize(m_DOF);
75 m_CurrentAngularVel.resize(m_DOF);
79 for (
int i = 0; i < m_DOF; i++)
81 std::ostringstream os;
82 os <<
"ID_Module_Number" << i + 1;
87 m_CurrentAngles[i] = 0.0;
88 m_CurrentAngularVel[i] = 0.0;
89 m_CurrentAngularMaxVel[i] = m_maxVel[i];
90 m_CurrentAngularMaxAccel[i] = m_maxAcc[i];
100 pthread_mutex_init(&m_Movement_Mutex, NULL);
101 pthread_mutex_init(&m_Angles_Mutex, NULL);
102 pthread_mutex_init(&m_AngularVel_Mutex, NULL);
104 m_SimThreadID = (pthread_t*)malloc(m_DOF *
sizeof(pthread_t));
105 m_MovementInProgress.resize(m_DOF);
107 for (
int i = 0; i < m_DOF; i++)
109 m_MovementInProgress[i] =
false;
111 m_SimThreadArgs[i]->cubeSimPtr =
this;
112 m_SimThreadArgs[i]->cubeID = i;
118 m_Initialized =
true;
127 for (
int i = 0; i < m_DOF; i++)
129 delete m_SimThreadArgs[i];
131 free(m_SimThreadArgs);
141 pthread_mutex_lock(&m_Angles_Mutex);
143 result.resize(m_DOF);
144 for (
int i; i < m_DOF; i++)
145 result[i] = m_CurrentAngles[i] *
DEG;
149 pthread_mutex_unlock(&m_Angles_Mutex);
162 pthread_mutex_lock(&m_AngularVel_Mutex);
164 result.resize(m_DOF);
165 result = m_CurrentAngularVel;
169 pthread_mutex_unlock(&m_AngularVel_Mutex);
178 pthread_mutex_lock(&m_Angles_Mutex);
181 m_CurrentAngles = Angles;
185 pthread_mutex_unlock(&m_Angles_Mutex);
193 pthread_mutex_lock(&m_AngularVel_Mutex);
196 m_CurrentAngularVel = AngularVel;
200 pthread_mutex_unlock(&m_AngularVel_Mutex);
208 std::cout <<
"Starting MoveJointSpaceSync(Jointd Angle) ... " << endl;
209 std::vector<double> targetRAD;
210 targetRAD.resize(m_DOF);
211 for (
int i = 0; i < m_DOF; i++)
212 targetRAD[i] = target[i] /
DEG;
215 std::vector<double> acc(m_DOF);
216 std::vector<double> vel(m_DOF);
225 std::vector<double> posnow;
226 if (getConfig(posnow) ==
false)
229 std::vector<double> velnow;
230 if (getJointVelocities(velnow) ==
false)
233 std::vector<double> times(DOF);
235 for (
int i = 0; i < DOF; i++)
237 RampCommand rm(posnow[i], velnow[i], targetRAD[i], m_maxAcc[i], m_maxVel[i]);
244 double max = times[0];
246 for (
int i = 1; i < m_DOF; i++)
255 RampCommand rm_furthest(posnow[furthest], velnow[furthest], targetRAD[furthest], m_maxAcc[furthest],
258 double T1 = rm_furthest.
T1();
259 double T2 = rm_furthest.
T2();
260 double T3 = rm_furthest.
T3();
266 acc[furthest] = m_maxAcc[furthest];
267 vel[furthest] = m_maxVel[furthest];
269 for (
int i = 0; i < DOF; i++)
288 startSimulatedMovement(targetRAD);
307 double vm = (dtheta < 0) ? -v : v;
308 double am = (dtheta < 0) ? -a : a;
313 double T1 = (vm - vnow) / am;
315 double dtheta1 = vnow * T1 + 0.5 * am * T1 * T1;
322 double dtheta3 = 0.5 * vm * T3;
325 double dtheta2 = dtheta - dtheta1 - dtheta3;
327 double T2 = dtheta2 / vm;
358 for (
int i = 0; i < m_DOF; i++)
359 setStatusMoving(i,
false);
367 for (
int i = 0; i < m_DOF; i++)
369 m_CurrentAngularMaxVel[i] = radpersec;
376 for (
int i = 0; i < m_DOF; i++)
378 m_CurrentAngularMaxVel[i] = radpersec[i];
389 for (
int i = 0; i < m_DOF; i++)
391 m_CurrentAngularMaxAccel[i] = radPerSecSquared;
400 for (
int i = 0; i < m_DOF; i++)
402 m_CurrentAngularMaxAccel[i] = radPerSecSquared[i];
412 bool isMoving =
false;
415 pthread_mutex_lock(&m_Movement_Mutex);
417 for (
int i = 0; i < m_DOF; i++)
419 if (m_MovementInProgress[i])
428 pthread_mutex_unlock(&m_Movement_Mutex);
437 bool isMoving =
false;
440 pthread_mutex_lock(&m_Movement_Mutex);
442 if (m_MovementInProgress[cubeNo])
449 pthread_mutex_unlock(&m_Movement_Mutex);
458 pthread_mutex_lock(&m_Movement_Mutex);
461 m_MovementInProgress[cubeNo] = moving;
464 pthread_mutex_unlock(&m_Movement_Mutex);
506 int cubeNo = args->
cubeID;
512 float t1, t2, t, tges;
514 t1 = t2 = t = tges = 0;
517 std::vector<double> currAngles;
519 std::vector<double> currVels;
522 double deltaAngle = targetAngle - currAngles[cubeNo];
525 t1 = maxVel / maxAccel;
528 t2 =
abs(deltaAngle) / maxVel - t1;
534 if (
abs(deltaAngle) > maxVel * maxVel / maxAccel)
541 t = sqrt(
abs(deltaAngle) / (maxAccel));
550 maxAccel = -maxAccel;
553 std::cout <<
"maxVel: " << maxVel <<
"; maxAccel: " << maxAccel <<
"; t1 [ms]: " << t1 <<
"; t2 [ms]: " << t2
554 <<
"; deltaAngle[rad]" << deltaAngle << endl;
558 double simulatedTime = 0.0;
560 float currDeltaAngle = deltaAngle;
562 if (
abs(deltaAngle) > 0.001)
564 while ((simulatedTime <= tges) && cubeSimPtr->
getStatusMoving(cubeNo) )
569 simulatedTime += SIM_CLOCK_FREQUENCY / 1000;
573 double deltaPhi = 0.0;
576 if (
abs(deltaAngle) > maxVel * maxVel /
abs(maxAccel))
578 if (simulatedTime < t1)
580 deltaPhi = 0.5 * maxAccel * (n * deltaT * n * deltaT - (n - 1) * deltaT * (n - 1) * deltaT);
581 currVels[cubeNo] = maxAccel * n * deltaT;
582 std::cout <<
"Phase 1, maxVel ->";
584 else if ((t1 < simulatedTime) && (simulatedTime < t1 + t2))
586 deltaPhi = maxVel * deltaT;
587 currVels[cubeNo] = maxVel;
588 std::cout <<
"Phase 2, maxVel ->";
590 else if ((simulatedTime > t1 + t2) && (simulatedTime < 2 * t1 + t2))
593 deltaPhi = maxVel * deltaT -
594 0.5 * maxAccel * ((n * deltaT - (t1 + t2)) * (n * deltaT - (t1 + t2)) -
595 ((n - 1) * deltaT - (t1 + t2)) * ((n - 1) * deltaT - (t1 + t2)));
596 currVels[cubeNo] = maxVel - maxAccel * (simulatedTime - (t1 + t2));
597 std::cout <<
"Phase 3, maxVel ->";
602 currVels[cubeNo] = 0.0;
603 std::cout <<
"Phase 4, maxVel ->";
609 if (simulatedTime < t)
612 deltaPhi = 0.5 * maxAccel * (n * deltaT * n * deltaT - (n - 1) * deltaT * (n - 1) * deltaT);
613 currVels[cubeNo] = maxAccel * simulatedTime;
614 std::cout <<
"Phase 1 ->";
616 else if ((simulatedTime > t) && (simulatedTime <= 2 * t))
619 deltaPhi = maxAccel * t * deltaT - 0.5 * maxAccel * ((n * deltaT - t) * (n * deltaT - t) - ((n - 1) * deltaT - t) * ((n - 1) * deltaT - t));
620 currVels[cubeNo] = maxAccel * t - maxAccel * (simulatedTime - t);
621 std::cout <<
"Phase 2 ->";
626 currVels[cubeNo] = 0.0;
627 std::cout <<
"Phase 3 ->" << t <<
"\n";
632 currAngles[cubeNo] = currAngles[cubeNo] + deltaPhi;
633 currDeltaAngle = targetAngle - currAngles[cubeNo];
645 currVels[cubeNo] = 0.0;
656 std::cout <<
"startSimulatedMovement: Movement already in progress, preemption not implemented yet! Aborting .." 663 for (
int i = 0; i < m_DOF; i++)
665 setStatusMoving(i,
true);
666 m_SimThreadArgs[i]->targetAngle = target[i];
667 pthread_create(&m_SimThreadID[i], NULL,
SimThreadRoutine, (
void*)m_SimThreadArgs[i]);
676 warten.tv_sec = milliseconds / 1000;
677 warten.tv_nsec = (milliseconds % 1000) * 1000000;
679 nanosleep(&warten, &gewartet);
bool getConfig(std::vector< double > &result)
Returns the current Joint Angles.
bool statusDec()
Returns true if any of the Joints are decelerating.
void setCurrentAngles(std::vector< double > Angles)
void setCurrentJointVelocities(std::vector< double > Angles)
bool setMaxVelocity(double radpersec)
Sets the maximum angular velocity (rad/s) for the Joints, use with care! A Value of 0...
bool MoveJointSpaceSync(const std::vector< double > &Angle)
same as MoveJointSpace, but final angles should by reached simultaniously! Returns the time that the ...
void millisleep(unsigned int milliseconds) const
double timeRampMove(double dtheta, double vnow, double v, double a)
Tells the Modules not to start moving until PCubel_startMotionAll is called.
#define PCTRL_CHECK_INITIALIZED()
bool statusMoving()
Returns true if any of the Joints are still moving Should also return true if Joints are accelerating...
bool setMaxAcceleration(double radPerSecSquared)
Sets the maximum angular acceleration (rad/s^2) for the Joints, use with care! A Value of 0...
std::vector< double > GetMaxAcc()
Gets the max. angular accelerations (rad/s^2) for the joints.
virtual double getTotalTime()
returns the planned total time for the movement (in seconds)
std::vector< double > getCurrentAngularMaxAccel()
static void calculateAV(double x0, double v0, double xtarget, double time, double T3, double amax, double vmax, double &a, double &v)
Calculate the necessary a and v of a rampmove, so that the move will take the desired time...
int startSimulatedMovement(std::vector< double > target)
std::vector< double > getCurrentAngularMaxVel()
bool Init(PowerCubeCtrlParams *params)
void * SimThreadRoutine(void *threadArgs)
void setStatusMoving(int cubeNo, bool moving)
bool Stop()
Stops the Manipulator immediately.
PowerCubeSim * cubeSimPtr
virtual double T1()
Return the times of the different phases of the ramp move.
bool getJointVelocities(std::vector< double > &result)
Returns the current Angular velocities (Rad/s)
std::vector< double > GetMaxVel()
Gets the max. angular velocities (rad/s) for the joints.
bool MoveVel(const std::vector< double > &vel)
Moves all cubes by the given velocities.
bool statusAcc()
Returs true if any of the Joints are accelerating.
Parameters for cob_powercube_chain.
#define SIM_CLOCK_FREQUENCY
bool getStatusMoving(int cubeNo) const