You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
288 lines
8.8 KiB
288 lines
8.8 KiB
/*
|
|
* VisualServoing is a tutorial program for introducing students to
|
|
* robotics.
|
|
*
|
|
* Copyright 2009, 2010 Kevin Quigley <kevin.quigley@gmail.com> and
|
|
* Marsette Vona <vona@ccs.neu.edu>
|
|
*
|
|
* VisualServoing is free software: you can redistribute it andor modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* VisualServoing is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* as the file COPYING along with VisualServoing. If not, see
|
|
* <http://www.gnu.org/licenses/>.
|
|
*/
|
|
#include "ArmControl.hpp"
|
|
|
|
#include <boost/numeric/ublas/io.hpp>
|
|
#include <boost/lexical_cast.hpp>
|
|
#include <iostream>
|
|
#include <math.h>
|
|
#include <limits.h>
|
|
#include <stdexcept>
|
|
#include <unistd.h>
|
|
#include <stdio.h>
|
|
|
|
/*!
|
|
* Initialize control using the SSC32Controller. Setup the saved park
|
|
* command and the arm to park.
|
|
*
|
|
* \param s The SSC32Controller.
|
|
* \param slave If true, the WRIST joint depends on the values of
|
|
* SHOULDER and ELBOW to remain horizontal.
|
|
*/
|
|
ArmControl::ArmControl(SSC32Controller& s, bool slave)
|
|
: control(&s),
|
|
slave_theta3(slave),
|
|
position(NUM_JOINTS), offset(NUM_JOINTS),
|
|
max_angle(NUM_JOINTS), min_angle(NUM_JOINTS),
|
|
set_busy(false), max_rate_set(false),
|
|
park_position(NUM_JOINTS)
|
|
{
|
|
|
|
for (unsigned int i = 0; i < position.size(); i++) position(i) = 0.0;
|
|
for (unsigned int i = 0; i < offset.size(); i++) offset(i) = 0;
|
|
|
|
park_position(0) = pwmToTheta(1500);
|
|
park_position(1) = pwmToTheta(2100);
|
|
park_position(2) = pwmToTheta(2000);
|
|
park_position(3) = pwmToTheta(2400);
|
|
park_position(4) = pwmToTheta(1800);
|
|
std::cout << "park position: " << park_position << std::endl;
|
|
|
|
park_cmd.addMove(Arm::BASE, 1500); // values on p31
|
|
park_cmd.addMove(Arm::SHOULDER, 2100);
|
|
park_cmd.addMove(Arm::ELBOW, 2000);
|
|
park_cmd.addMove(Arm::WRIST, 2400);
|
|
park_cmd.addMove(Arm::GRIP, 1800);
|
|
park_cmd.minTime(4000); // 4 sec
|
|
|
|
control->move(park_cmd);
|
|
//park(); // TBD - same, but sets position?
|
|
}
|
|
|
|
/*!
|
|
* Destroy this objects, releasing all servos from control on exit.
|
|
*/
|
|
ArmControl::~ArmControl(void) {
|
|
// printf("parking\n"); //TBD this causes ugly delay on app shutdown
|
|
// control->move(park_cmd);
|
|
// sleep(5);
|
|
printf("release servos\n");
|
|
control->releaseAll();
|
|
// sleep(1);
|
|
}
|
|
|
|
/*!
|
|
* Move by the specified delta angle with relation to the current
|
|
* angular position of the joints.
|
|
*
|
|
* \param[in] dtheta The requested change to the current joint angles
|
|
* (radians).
|
|
*/
|
|
void
|
|
ArmControl::moveDelta(const ublas::vector<double>& dtheta) {
|
|
|
|
for (unsigned int i = 0; i < dtheta.size(); i++)
|
|
if (dtheta(i) != dtheta(i))
|
|
throw std::runtime_error(std::string("commanded dtheta NaN at index ")+
|
|
boost::lexical_cast<std::string>(i));
|
|
|
|
for (unsigned int i = 0; i < dtheta.size(); i++)
|
|
if(fabs(dtheta(i)) == std::numeric_limits<double>::infinity())
|
|
throw std::runtime_error(std::string("commanded dtheta inf at index ")+
|
|
boost::lexical_cast<std::string>(i));
|
|
|
|
for (unsigned int i = 0; i < (dtheta.size() < position.size()
|
|
? dtheta.size() : position.size()); i++)
|
|
position(i) += dtheta(i);
|
|
|
|
moveToPosition(position, 50);
|
|
}
|
|
|
|
/*!
|
|
* Move to the specified joint angle position taking at least time to
|
|
* move. The move may take longer than time to complete.
|
|
*
|
|
* \param[in] pos The requested joint position (radians).
|
|
* \param[in] time The minimum time of movement (milliseconds).
|
|
*/
|
|
void
|
|
ArmControl::moveToPosition(const ublas::vector<double>& pos,
|
|
unsigned int time) {
|
|
|
|
for (unsigned int i = 0; i < pos.size(); i++)
|
|
if (pos(i) != pos(i))
|
|
throw std::runtime_error(std::string("commanded position NaN at index ")+
|
|
boost::lexical_cast<std::string>(i));
|
|
|
|
for (unsigned int i = 0; i < pos.size(); i++)
|
|
if(fabs(pos(i)) == std::numeric_limits<double>::infinity())
|
|
throw std::runtime_error(std::string("commanded position inf at index ")+
|
|
boost::lexical_cast<std::string>(i));
|
|
|
|
position = pos;
|
|
|
|
printf("commanded position [");
|
|
for (unsigned int i = 0; i < position.size(); i++)
|
|
printf("%10f,", position(i));
|
|
printf("]\n");
|
|
|
|
if (slave_theta3)
|
|
position(3) = M_PI_2 - position(1) - position(2);
|
|
|
|
for (unsigned int i = 0; i < position.size(); i++) {
|
|
if (position(i) < min_angle(i))
|
|
position(i) = min_angle(i);
|
|
else if (position(i) > max_angle(i))
|
|
position(i) = max_angle(i);
|
|
}
|
|
|
|
printf("actual position [");
|
|
for (unsigned int i = 0; i < position.size(); i++)
|
|
printf("%10f,", position(i));
|
|
printf("]\n");
|
|
|
|
std::cout << "offsets: " << offset << std::endl;
|
|
Command cmd;
|
|
cmd.addMove(Arm::BASE,
|
|
thetaToPwm(position(0), offset(Arm::BASE)),
|
|
max_rate_set ? max_rate : Command::ANY_SPEED);
|
|
cmd.addMove(Arm::SHOULDER,
|
|
thetaToPwm(position(1), offset(Arm::SHOULDER)),
|
|
max_rate_set ? max_rate : Command::ANY_SPEED);
|
|
cmd.addMove(Arm::ELBOW,
|
|
thetaToPwm(position(2), offset(Arm::ELBOW)),
|
|
max_rate_set ? max_rate : Command::ANY_SPEED);
|
|
if (slave_theta3)
|
|
cmd.addMove(Arm::WRIST,
|
|
thetaToPwm(M_PI_2 - position(1) - position(2),
|
|
offset(Arm::WRIST)),
|
|
max_rate_set ? max_rate : Command::ANY_SPEED);
|
|
else cmd.addMove(Arm::WRIST, thetaToPwm(position(3), offset(Arm::WRIST)));
|
|
//cmd.addMove(Arm::GRIP, thetaToPwm(position(4), offset(Arm::GRIP)));
|
|
|
|
if (time != 0) cmd.minTime(time);
|
|
|
|
control->move(cmd);
|
|
}
|
|
|
|
/*!
|
|
* Convert an angle to a PWM length, accounting for a calibration offset.
|
|
*
|
|
* \param[in] theta The angle to convert (radians).
|
|
* \param[in] offset Calibration PWM offset (microseconds).
|
|
*
|
|
* \return The equivalent PWM length (microseconds).
|
|
*/
|
|
unsigned int
|
|
ArmControl::thetaToPwm(double theta, int offset) {
|
|
// scale theta to -PI:PI
|
|
//while (theta < -M_PI) theta -= ((int)theta%(int)M_PI)*M_PI;
|
|
//while (theta > M_PI) theta -= ((int)theta%(int)M_PI)*M_PI;
|
|
|
|
// PI/2:-PI/2 == 600:2400
|
|
// negate theta because +theta is low PWM
|
|
unsigned int pwm = trunc((-theta + M_PI_2) * 1800 / M_PI) + 600 + offset;
|
|
if (pwm < 500) return 500;
|
|
if (pwm > 2500) return 2500;
|
|
return pwm;
|
|
}
|
|
|
|
/*!
|
|
* Convert the PWM length to an angle, accounting for a calibration
|
|
* offset.
|
|
*
|
|
* \param[in] pwm The PWM length (microseconds).
|
|
* \param[in] offset Calibration PWM offset (microseconds).
|
|
*
|
|
* \return The equivalent angle (radians).
|
|
*/
|
|
double
|
|
ArmControl::pwmToTheta(unsigned int pwm, int offset) {
|
|
// PI/2:-PI/2 == 600:2400
|
|
// theta value is negative because low PWM is +theta
|
|
return -((((double)pwm - 600.0 - offset) * M_PI / 1800.0) - M_PI_2);
|
|
}
|
|
|
|
/*!
|
|
* Command the arm to a saved \a home position.
|
|
*/
|
|
void
|
|
ArmControl::home(void) {
|
|
bool temp = slave_theta3;
|
|
slave_theta3 = true;
|
|
ublas::vector<double>pos(ublas::zero_vector<double>(NUM_JOINTS));
|
|
pos(0) = 0.0;
|
|
pos(1) = -M_PI_4;
|
|
pos(2) = M_PI_2;
|
|
moveToPosition(pos,1000);
|
|
slave_theta3 = temp;
|
|
}
|
|
|
|
/*!
|
|
* Command the grip to close to a position which would grab the ball.
|
|
*/
|
|
void
|
|
ArmControl::grabBall(void) {
|
|
Command cmd;
|
|
cmd.addMove(Arm::GRIP, 1350 + offset(Arm::GRIP), 500);
|
|
control->move(cmd);
|
|
}
|
|
|
|
/*!
|
|
* Command the grip to close to a position which would grab a
|
|
* specially designed marker-holder for the class exercise.
|
|
*/
|
|
void
|
|
ArmControl::grabMarker(void) {
|
|
Command cmd;
|
|
cmd.addMove(Arm::GRIP, 1350 + offset(Arm::GRIP) - 200, 500);
|
|
control->move(cmd);
|
|
}
|
|
|
|
/*!
|
|
* Open the gripper.
|
|
*/
|
|
void
|
|
ArmControl::openGrip(void) {
|
|
Command cmd;
|
|
cmd.addMove(Arm::GRIP, 1350 + offset(Arm::GRIP) + 500, 500);
|
|
control->move(cmd);
|
|
}
|
|
|
|
/*!
|
|
* Stop movement of the arm.
|
|
* \b NOTE: Although the SSC-32 documentation lists: <tt>"<esc> =
|
|
* Cancel the current command, ASCII 27."</tt> (e.g.0x1b) this does
|
|
* not stop single or group moves in progress. The only way to
|
|
* effectively do this is to read each servo position and then command
|
|
* that position again to override any existing command. Since
|
|
* querying is not currently implemented, calling stop() releases all
|
|
* servo power.
|
|
*/
|
|
void
|
|
ArmControl::stop(void) {
|
|
control->releaseAll();
|
|
}
|
|
|
|
/*!
|
|
* \param[in,out] v The provided vector of degree values, converted in
|
|
* place to radian values.
|
|
*/
|
|
void
|
|
ArmControl::degreesToRadians(ublas::vector<double>& v) { v *= M_PI / 180.0; }
|
|
|
|
/*!
|
|
* \param[in,out] v The provided vector of radian values, converted in
|
|
* place to degree values.
|
|
*/
|
|
void
|
|
ArmControl::radiansToDegrees(ublas::vector<double>& v) { v *= 180.0 / M_PI; }
|