Update of /cvsroot/playerstage/code/player/server/drivers/limb
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv10946/server/drivers/limb
Modified Files:
eeDHcontroller.cc
Log Message:
applied Toby's patch to replace fixed-size arrays
Index: eeDHcontroller.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/limb/eeDHcontroller.cc,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** eeDHcontroller.cc 23 Aug 2007 19:58:44 -0000 1.3
--- eeDHcontroller.cc 1 Nov 2007 22:16:19 -0000 1.4
***************
*** 20,24 ****
*/
/*
! Desc: Calculates the joint commands for a given End Effector pose using
ROBOOP's Inverse Kinematics algorithms.
Author: Radu Bogdan Rusu and Dan Pojar
--- 20,24 ----
*/
/*
! Desc: Calculates the joint commands for a given End Effector pose using
ROBOOP's Inverse Kinematics algorithms.
Author: Radu Bogdan Rusu and Dan Pojar
***************
*** 30,34 ****
/** @{ */
/** @defgroup driver_eeDHcontroller eeDHcontroller
! * @brief Calculates the joint commands for a given End Effector pose using
ROBOOP's Inverse Kinematics algorithms.
--- 30,34 ----
/** @{ */
/** @defgroup driver_eeDHcontroller eeDHcontroller
! * @brief Calculates the joint commands for a given End Effector pose using
ROBOOP's Inverse Kinematics algorithms.
***************
*** 69,73 ****
- PLAYER_LIMB_REQ_SPEED
!
@par Configuration file options
--- 69,73 ----
- PLAYER_LIMB_REQ_SPEED
!
@par Configuration file options
***************
*** 75,79 ****
- The number of joints that we provide DH parameters for (should be the
same as the number of actuators the actarray interface provides).
!
- jointX_DH (integer tuple)
- [ R/P theta d a alfa th_min th_max ] - DH parameters for joint X
--- 75,79 ----
- The number of joints that we provide DH parameters for (should be the
same as the number of actuators the actarray interface provides).
!
- jointX_DH (integer tuple)
- [ R/P theta d a alfa th_min th_max ] - DH parameters for joint X
***************
*** 81,86 ****
- error_pos (float)
- Default: 0
! - User allowed error value in position in degrees. This is needed for
joints
! who do not change their state (eg. they remain idle) when a command is
given
and the joint is already in that position.
--- 81,86 ----
- error_pos (float)
- Default: 0
! - User allowed error value in position in degrees. This is needed for joints
! who do not change their state (eg. they remain idle) when a command is
given
and the joint is already in that position.
***************
*** 90,94 ****
Valid values: 0 (disabled), 1 (enabled).
! @par Example
@verbatim
--- 90,94 ----
Valid values: 0 (disabled), 1 (enabled).
! @par Example
@verbatim
***************
*** 111,115 ****
# Allowed positioning error in degrees
error_pos 0.01
!
# Enable debug mode
debug 1
--- 111,115 ----
# Allowed positioning error in degrees
error_pos 0.01
!
# Enable debug mode
debug 1
***************
*** 149,153 ****
// This method will be invoked on each incoming message
! virtual int ProcessMessage (QueuePointer &resp_queue,
player_msghdr * hdr,
void * data);
--- 149,153 ----
// This method will be invoked on each incoming message
! virtual int ProcessMessage (QueuePointer &resp_queue,
player_msghdr * hdr,
void * data);
***************
*** 170,174 ****
Matrix DHMatrixModel;
unsigned int nr_joints;
!
// Create two Robot objects (for IK and for FK)
Robot robot_IK, robot_FK;
--- 170,174 ----
Matrix DHMatrixModel;
unsigned int nr_joints;
!
// Create two Robot objects (for IK and for FK)
Robot robot_IK, robot_FK;
***************
*** 183,189 ****
// Send commands to the actarray
void CommandJoints (ColumnVector q_cmd);
!
// Keeping track of whether joints are still moving or not
! int actarray_state[PLAYER_ACTARRAY_NUM_ACTUATORS];
// Threads used for homing or moving the actarray
--- 183,189 ----
// Send commands to the actarray
void CommandJoints (ColumnVector q_cmd);
!
// Keeping track of whether joints are still moving or not
! int *actarray_state;
// Threads used for homing or moving the actarray
***************
*** 198,202 ****
int debug;
!
// Allowed positioning error in degrees
float error_pos;
--- 198,202 ----
int debug;
!
// Allowed positioning error in degrees
float error_pos;
***************
*** 213,217 ****
////////////////////////////////////////////////////////////////////////////////
! // Registers the driver in the driver table. Called from the
// player_driver_init function that the loader looks for
void
--- 213,217 ----
////////////////////////////////////////////////////////////////////////////////
! // Registers the driver in the driver table. Called from the
// player_driver_init function that the loader looks for
void
***************
*** 246,250 ****
DHMatrixModel = Matrix (6, 23);
DHMatrixModel = 0;
!
for (i = 0; i < nr_joints; i++)
{
--- 246,252 ----
DHMatrixModel = Matrix (6, 23);
DHMatrixModel = 0;
!
! actarray_state = new int[nr_joints];
!
for (i = 0; i < nr_joints; i++)
{
***************
*** 257,272 ****
DHMatrixModel (i+1, j+1) = bla;
}
!
// Set the initial status of joints (IDLE)
actarray_state[i] = PLAYER_ACTARRAY_ACTSTATE_IDLE;
}
!
// Instantiate the robot(s) with the DH parameter matrix
robot_IK = Robot (DHMatrixModel);
robot_FK = Robot (DHMatrixModel);
q_cmd = ColumnVector (nr_joints);
!
debug = cf->ReadInt (section, "debug", 0);
!
// Allowed positioning error in degrees
error_pos = cf->ReadFloat (section, "error_pos", 1.0);
--- 259,274 ----
DHMatrixModel (i+1, j+1) = bla;
}
!
// Set the initial status of joints (IDLE)
actarray_state[i] = PLAYER_ACTARRAY_ACTSTATE_IDLE;
}
!
// Instantiate the robot(s) with the DH parameter matrix
robot_IK = Robot (DHMatrixModel);
robot_FK = Robot (DHMatrixModel);
q_cmd = ColumnVector (nr_joints);
!
debug = cf->ReadInt (section, "debug", 0);
!
// Allowed positioning error in degrees
error_pos = cf->ReadFloat (section, "error_pos", 1.0);
***************
*** 278,281 ****
--- 280,287 ----
EEDHController::~EEDHController()
{
+ if (actarray_state != NULL)
+ {
+ delete[] actarray_state;
+ }
}
***************
*** 301,305 ****
// Start the device thread
StartThread ();
!
return (0);
}
--- 307,311 ----
// Start the device thread
StartThread ();
!
return (0);
}
***************
*** 315,319 ****
pthread_cancel (a_th_cmd);
pthread_cancel (a_th_home);
!
this->actarray_device->Unsubscribe (this->InQueue);
--- 321,325 ----
pthread_cancel (a_th_cmd);
pthread_cancel (a_th_home);
!
this->actarray_device->Unsubscribe (this->InQueue);
***************
*** 326,335 ****
// Main function for device thread
void
! EEDHController::Main ()
{
timespec sleepTime = {0, 1000};
command_thread = homing_thread = false;
!
// The main loop; interact with the device here
while (true)
--- 332,341 ----
// Main function for device thread
void
! EEDHController::Main ()
{
timespec sleepTime = {0, 1000};
command_thread = homing_thread = false;
!
// The main loop; interact with the device here
while (true)
***************
*** 350,357 ****
////////////////////////////////////////////////////////////////////////////////
// Compute the joint commands using IK
! int
! EEDHController::ComputeQ (unsigned int dof,
! double x, double y, double z,
! double roll, double pitch, double yaw,
ColumnVector &q)
{
--- 356,363 ----
////////////////////////////////////////////////////////////////////////////////
// Compute the joint commands using IK
! int
! EEDHController::ComputeQ (unsigned int dof,
! double x, double y, double z,
! double roll, double pitch, double yaw,
ColumnVector &q)
{
***************
*** 373,377 ****
// Call RoboOp's inv_kin
bool converged;
!
q = robot_IK.inv_kin (result, 1, dof, converged);
if (q.is_zero () && !converged)
--- 379,383 ----
// Call RoboOp's inv_kin
bool converged;
!
q = robot_IK.inv_kin (result, 1, dof, converged);
if (q.is_zero () && !converged)
***************
*** 387,391 ****
{
float rad = 0;
!
if (a == 0)
rad = b < 0 ? -M_PI/2 : M_PI/2;
--- 393,397 ----
{
float rad = 0;
!
if (a == 0)
rad = b < 0 ? -M_PI/2 : M_PI/2;
***************
*** 403,407 ****
////////////////////////////////////////////////////////////////////////////////
// Compute the End-Effector pose from the joint's position using FK
! ColumnVector
EEDHController::ComputeEEPose (ColumnVector q_cmd)
{
--- 409,413 ----
////////////////////////////////////////////////////////////////////////////////
// Compute the End-Effector pose from the joint's position using FK
! ColumnVector
EEDHController::ComputeEEPose (ColumnVector q_cmd)
{
***************
*** 415,420 ****
// Roll/X, Pitch/Y, Yaw/Z
ret(1) = m_atan2 (position (3, 3), position (3, 2));
! ret(2) = m_atan2 (sqrt ((position (3, 2) * position (3, 2)) +
! (position (3, 3) * position (3, 3))),
-position (3, 1));
ret(3) = m_atan2 (position(1, 1), position (2, 1));
--- 421,426 ----
// Roll/X, Pitch/Y, Yaw/Z
ret(1) = m_atan2 (position (3, 3), position (3, 2));
! ret(2) = m_atan2 (sqrt ((position (3, 2) * position (3, 2)) +
! (position (3, 3) * position (3, 3))),
-position (3, 1));
ret(3) = m_atan2 (position(1, 1), position (2, 1));
***************
*** 436,440 ****
{
assert (nr_joints == actarray_data.actuators_count);
!
// Set the vector state of the actuators
limb_data.state = PLAYER_LIMB_STATE_BRAKED;
--- 442,446 ----
{
assert (nr_joints == actarray_data.actuators_count);
!
// Set the vector state of the actuators
limb_data.state = PLAYER_LIMB_STATE_BRAKED;
***************
*** 458,471 ****
// Fill the limb data structure with values and publish it
! limb_data.position.px = pose (4);
! limb_data.position.py = pose (5);
! limb_data.position.pz = pose (6);
!
! limb_data.approach.px = -1; limb_data.approach.py = -1;
limb_data.approach.pz = -1;
!
! limb_data.orientation.px = pose (1);
! limb_data.orientation.py = pose (2);
! limb_data.orientation.pz = pose (3);
!
Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_LIMB_DATA,
&limb_data, sizeof (limb_data), NULL);
--- 464,477 ----
// Fill the limb data structure with values and publish it
! limb_data.position.px = pose (4);
! limb_data.position.py = pose (5);
! limb_data.position.pz = pose (6);
!
! limb_data.approach.px = -1; limb_data.approach.py = -1;
limb_data.approach.pz = -1;
!
! limb_data.orientation.px = pose (1);
! limb_data.orientation.py = pose (2);
! limb_data.orientation.pz = pose (3);
!
Publish (device_addr, PLAYER_MSGTYPE_DATA, PLAYER_LIMB_DATA,
&limb_data, sizeof (limb_data), NULL);
***************
*** 492,502 ****
player_actarray_home_cmd_t cmd;
int i;
!
// Home one joint at a time
for (i = nr_joints-1; i > -1; i--)
! {
cmd.joint = i;
! this->actarray_device->PutMsg (this->InQueue,
PLAYER_MSGTYPE_CMD,
PLAYER_ACTARRAY_CMD_HOME,
--- 498,508 ----
player_actarray_home_cmd_t cmd;
int i;
!
// Home one joint at a time
for (i = nr_joints-1; i > -1; i--)
! {
cmd.joint = i;
! this->actarray_device->PutMsg (this->InQueue,
PLAYER_MSGTYPE_CMD,
PLAYER_ACTARRAY_CMD_HOME,
***************
*** 509,514 ****
while (! (
! (p_state != c_state)
! &&
(c_state != PLAYER_ACTARRAY_ACTSTATE_MOVING)
))
--- 515,520 ----
while (! (
! (p_state != c_state)
! &&
(c_state != PLAYER_ACTARRAY_ACTSTATE_MOVING)
))
***************
*** 557,561 ****
player_actarray_position_cmd_t cmd;
int i;
!
// Write the commands on screen if debug enabled
if (debug)
--- 563,567 ----
player_actarray_position_cmd_t cmd;
int i;
!
// Write the commands on screen if debug enabled
if (debug)
***************
*** 569,573 ****
// Position one joint at a time
for (i = 0; i < q_cmd.nrows (); i++)
! {
cmd.joint = i;
cmd.position = q_cmd (i + 1);
--- 575,579 ----
// Position one joint at a time
for (i = 0; i < q_cmd.nrows (); i++)
! {
cmd.joint = i;
cmd.position = q_cmd (i + 1);
***************
*** 579,584 ****
if (abs(cmd.position - actarray_data.actuators[i].position) < DTOR
(error_pos))
break;
!
! this->actarray_device->PutMsg (this->InQueue,
PLAYER_MSGTYPE_CMD,
PLAYER_ACTARRAY_CMD_POS,
--- 585,590 ----
if (abs(cmd.position - actarray_data.actuators[i].position) < DTOR
(error_pos))
break;
!
! this->actarray_device->PutMsg (this->InQueue,
PLAYER_MSGTYPE_CMD,
PLAYER_ACTARRAY_CMD_POS,
***************
*** 589,594 ****
while (! (
! (p_state != c_state)
! &&
(c_state != PLAYER_ACTARRAY_ACTSTATE_MOVING)
))
--- 595,600 ----
while (! (
! (p_state != c_state)
! &&
(c_state != PLAYER_ACTARRAY_ACTSTATE_MOVING)
))
***************
*** 598,602 ****
nanosleep (&sleepTime, NULL);
}
!
if (debug)
printf ("[done]\n");
--- 604,608 ----
nanosleep (&sleepTime, NULL);
}
!
if (debug)
printf ("[done]\n");
***************
*** 610,614 ****
// ProcessMessage function
int
! EEDHController::ProcessMessage (QueuePointer &resp_queue,
player_msghdr * hdr,
void * data)
--- 616,620 ----
// ProcessMessage function
int
! EEDHController::ProcessMessage (QueuePointer &resp_queue,
player_msghdr * hdr,
void * data)
***************
*** 620,626 ****
HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_LIMB_CMD_SETPOSITION);
HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_LIMB_CMD_HOME);
!
// First look whether we have incoming data from the actarray interface
! if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA,
PLAYER_ACTARRAY_DATA_STATE, actarray_addr))
{
--- 626,632 ----
HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_LIMB_CMD_SETPOSITION);
HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_LIMB_CMD_HOME);
!
// First look whether we have incoming data from the actarray interface
! if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA,
PLAYER_ACTARRAY_DATA_STATE, actarray_addr))
{
***************
*** 631,648 ****
// Set the desired position to the actarray driver
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_CMD,
PLAYER_LIMB_CMD_SETPOSE, device_addr))
{
! player_limb_setpose_cmd_t & command =
*reinterpret_cast<player_limb_setpose_cmd_t * > (data);
! int converged = ComputeQ (nr_joints, command.position.px,
! command.position.py, command.position.pz,
command.orientation.px, command.orientation.py,
command.orientation.pz, q_cmd);
if (converged != 0)
{
! PLAYER_WARN6 ("Couldn't find solution for %f,%f,%f/%f,%f,%f",
command.position.px,
! command.position.py, command.position.pz,
command.orientation.px,
command.orientation.py, command.orientation.pz);
// Do we actually need to send back a NACK ?
--- 637,654 ----
// Set the desired position to the actarray driver
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_CMD,
PLAYER_LIMB_CMD_SETPOSE, device_addr))
{
! player_limb_setpose_cmd_t & command =
*reinterpret_cast<player_limb_setpose_cmd_t * > (data);
! int converged = ComputeQ (nr_joints, command.position.px,
! command.position.py, command.position.pz,
command.orientation.px, command.orientation.py,
command.orientation.pz, q_cmd);
if (converged != 0)
{
! PLAYER_WARN6 ("Couldn't find solution for %f,%f,%f/%f,%f,%f",
command.position.px,
! command.position.py, command.position.pz,
command.orientation.px,
command.orientation.py, command.orientation.pz);
// Do we actually need to send back a NACK ?
***************
*** 656,672 ****
// Set the desired position to the actarray driver
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_CMD,
PLAYER_LIMB_CMD_SETPOSITION, device_addr))
{
! player_limb_setposition_cmd_t & command =
*reinterpret_cast<player_limb_setposition_cmd_t * > (data);
!
! int converged = ComputeQ (nr_joints, command.position.px,
command.position.py, command.position.pz, 0, 0,
0, q_cmd);
!
if (converged != 0)
{
// Do we actually need to send back a NACK ?
! PLAYER_WARN3 ("Couldn't find solution for %f,%f,%f",
command.position.px,
command.position.py, command.position.pz);
Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
hdr->subtype);
--- 662,678 ----
// Set the desired position to the actarray driver
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_CMD,
PLAYER_LIMB_CMD_SETPOSITION, device_addr))
{
! player_limb_setposition_cmd_t & command =
*reinterpret_cast<player_limb_setposition_cmd_t * > (data);
!
! int converged = ComputeQ (nr_joints, command.position.px,
command.position.py, command.position.pz, 0, 0,
0, q_cmd);
!
if (converged != 0)
{
// Do we actually need to send back a NACK ?
! PLAYER_WARN3 ("Couldn't find solution for %f,%f,%f",
command.position.px,
command.position.py, command.position.pz);
Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
hdr->subtype);
***************
*** 679,683 ****
// Home the limb (we do this by homing all the joints)
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_CMD,
PLAYER_LIMB_CMD_HOME, device_addr))
{
--- 685,689 ----
// Home the limb (we do this by homing all the joints)
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_CMD,
PLAYER_LIMB_CMD_HOME, device_addr))
{
***************
*** 690,694 ****
// POWER_REQ not implemented
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LIMB_REQ_POWER, device_addr))
{
--- 696,700 ----
// POWER_REQ not implemented
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LIMB_REQ_POWER, device_addr))
{
***************
*** 698,702 ****
// BRAKES_REQ not implemented
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LIMB_REQ_BRAKES, device_addr))
{
--- 704,708 ----
// BRAKES_REQ not implemented
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LIMB_REQ_BRAKES, device_addr))
{
***************
*** 706,710 ****
// GEOM_REQ not implemented
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LIMB_REQ_GEOM, device_addr))
{
--- 712,716 ----
// GEOM_REQ not implemented
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LIMB_REQ_GEOM, device_addr))
{
***************
*** 714,732 ****
// SPEED_REQ - Set the speed on all joints equal to the EE
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LIMB_REQ_SPEED, device_addr))
{
! player_limb_speed_req_t & cfg =
*reinterpret_cast<player_limb_speed_req_t *> (data);
!
int i;
player_actarray_speed_config_t act_cfg;
for (i = 0; i < q_cmd.nrows (); i++)
! {
act_cfg.joint = i;
act_cfg.speed = cfg.speed;
Message *msg;
! if (!(msg = this->actarray_device->Request (this->InQueue,
PLAYER_MSGTYPE_REQ,
PLAYER_ACTARRAY_REQ_SPEED,
--- 720,738 ----
// SPEED_REQ - Set the speed on all joints equal to the EE
! else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LIMB_REQ_SPEED, device_addr))
{
! player_limb_speed_req_t & cfg =
*reinterpret_cast<player_limb_speed_req_t *> (data);
!
int i;
player_actarray_speed_config_t act_cfg;
for (i = 0; i < q_cmd.nrows (); i++)
! {
act_cfg.joint = i;
act_cfg.speed = cfg.speed;
Message *msg;
! if (!(msg = this->actarray_device->Request (this->InQueue,
PLAYER_MSGTYPE_REQ,
PLAYER_ACTARRAY_REQ_SPEED,
***************
*** 739,743 ****
delete msg;
}
!
Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
return (0);
--- 745,749 ----
delete msg;
}
!
Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype);
return (0);
-------------------------------------------------------------------------
This SF.net email is sponsored by: Splunk Inc.
Still grepping through log files to find problems? Stop.
Now Search log events and configuration files using AJAX and a browser.
Download your FREE copy of Splunk now >> http://get.splunk.com/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
