Add speed getter and acceleration getter and setter functions

[metanav]

How can I achieve these:

• Get Speed
• Get/Set Acceleration (possible?)

Also, the speed() method parameter is time (ms ?) not the speed (rad/s).

[aentinger]

Hi @metanav ☕ 👋

I am highly interested in what you describe you intend to do over in #81 .

There already exists this draft PR #76 which provides a partial ROS2 driver for the Braccio++ written by me. You are kindly invited to extend it to make your application work.

As for the APIs: speed equals to run time right now. Run time is defined as the time which a servo takes to reach the next set point, i.e. if you are currently at 90° and you set 190° as a next target with a runtime of 100 ms (Braccio.speed(SERVO_ID, 100);) this means it will take the servo roughly 100 ms to cover those 100° going from 90° to 190°.

You'd be welcome to add an additional API which allows configuring speed on a rad/s basis.

The specification of the motors containing max. torque etc. are to be found here (section Tech Specs).

[metanav]

Hi @aentinger,

I have seen the PR you mentioned and already borrowed few things from it. I have implemented joint_states publisher and follow_joint_trajectory action server (to receive goals) using ROS2/microROS successfully. I am able to receive goals from the RViz via MoveIt2 at the carrier board MCU (Nano RP2040 connect). The issue is the arm pose/movement is not accurate due to the wrong constraints defined in the Braccio++ URDF. Also, to apply the goal trajectory points to the Braccio++ joints requires velocity and acceleration to move it as intended. Right now I am converting velocity to time using the similar calculation as you mentioned. I will try to implement the missing APIs but I need the Servos datasheet.

[aentinger]

The "datasheet" of the servos can be found via link in last line of my comment above. Sorry, I fixed the link. It should be obvious now.

[metanav]

Thanks @aentinger! I found the speed/torque limits via the link.

[aentinger]

Please consider adding your working example via PR to this repository 🙏

[aentinger]

Hi @metanav 👋

You are going to like the changes in #88 .

It's now possible to configured the desired angular velocity via setAngularVelocity. This change has been necessary on our side anyway, as so far there was no speed limitation and if you were setting position set-points with a large difference between them the arm could move quite unexpectedly fast.

[metanav]

Thanks @aentinger! I will test them soon.

Few questions:

• When you say angular velocity, do they take negative values, too? (Velocity: vector, Speed: scalar)
• The setTorque() method name is kinda misleading, it enables the torque (not setting the value). Also, the engage()/disengage() methods seem doing almost similar job?
• There is a SmartServoRegister SPEED_H/SPEED_L which can read run time speed. A method would be useful to access them but how to read them while moving the joints? Does the SmartServoClass use threads?

[aentinger]

When you say angular velocity, do they take negative values, too? (Velocity: vector, Speed: scalar)

No, there are still position set-points. You can merely define the angular velocity from one position set-point to the next.

The setTorque() method name is kinda misleading, it enables the torque (not setting the value). Also, the engage()/disengage() methods seem doing almost similar job?

It's true. However, you should not be directly using setTorque() at all. There's a reason its not part of the public API, while engage()/disengage() are.

Does the SmartServoClass use threads?

No. But there's a thread that periodically "pings" all servos via the SmartServoClass::ping API. In order to avoid any of the typical multi-threading problems each access to RS485 is locked via mutex.

There is a SmartServoRegister SPEED_H/SPEED_L which can read run time speed.

If you are in that deep already, surely you know how to add an method for reading this register? 😉 readWordCmd is your friend.

[metanav]

I've tested it on the hardware and the joints move with the set velocity value. We may need to extend this API call, though. In the follow_joint_trajectory message from ROS may have different velocities (rad/s) for the joints at a specific trajectory point.

Example Joint Trajectory:

header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: base_link
joint_names: 
- base_joint
- elbow_joint
- shoulder_joint
- wrist_pitch_joint
- wrist_roll_joint
positions [0] 
  2.6156
  2.2843
  2.8602
  2.8726
  1.5820
velocities [0]: 
  0.0000
  0.0000
  0.0000
  0.0000
  0.0000
accelerations [0]: 
  -0.9588
  0.0000
  0.0000
  0.0000
  0.0000
effort [0]: []
time_from_start:
  sec: 0
  nanosec: 0
positions [1] 
  2.5531
  2.2965
  2.8387
  2.9278
  1.6225
velocities [1]: 
  -0.2865
  0.0557
  -0.0986
  0.2532
  0.1860
accelerations [1]: 
  -0.8782
  0.1709
  -0.3023
  0.7762
  0.5702
effort [1]: []
time_from_start:
  sec: 0
  nanosec: 360874814
positions [2] 
  2.4907
  2.3086
  2.8173
  2.9829
  1.6631
velocities [2]: 
  -0.4000
  0.0778
  -0.1377
  0.3535
  0.2597
accelerations [2]: 
  0.0000
  0.0000
  0.0000
  0.0000
  0.0000
effort [2]: []
time_from_start:
  sec: 0
  nanosec: 516950736

[...]

I am thinking how can we implement and achieve the velocity at each position.

[metanav]

HI @aentinger,

The smaller Servo motors (gripper/wrist roll) move extremely slow in the comparision of other servo motors using the setAngularVelocity().

Example to reproduce:

#include <Braccio++.h>

void setup() {
  Braccio.begin();
  delay(500);
  Braccio.setAngularVelocity(100);
  Braccio.setMaxTorque(1000);

  float homePos[6]     = {100.0, 165.0, 165.0, 165.0, 165.0, 180.0};
  Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]);
}

void loop() {
}

[aentinger]

I imagine this to be because the internal control loop is different than for other servos (since the hardware is also different)? Have you tried with setting a higher angular velocity? I imagine it would be helpful to set a angular velocity per joint, instead one shared between all joints ... of course I gather you'd rather like to directly feed velocity instead of position as a input parameter. If so, can you prepare a PR for extending the API in that way?

[metanav]

By Increasing the velocity by 8 times Braccio.setAngularVelocity(800);, it moves faster but not that much fast it should be.
I will prepare a PR.

[aentinger]

Just curious, how's your PR on this issue doing?

[metanav]

Hi, @aentinger. Sorry for the long delay on this issue. It took a considerable time to fully comprehend ROS2/MoveIt2. Now I am working on applying the motion planning path trajectories to the joints. Below is one of the real-time trajectories segment.

joint_names: - 
- base
- elbow
- shoulder
- wrist_pitch
- wrist_roll
positions [0] 
  2.7053
  4.4450
  3.1689
  2.3569
  2.8000
velocities [0]: 
  -0.0000
  -0.0000
  0.0000
  0.0000
  -0.0000
accelerations [0]: 
  0.0000
  0.0000
  0.0000
  0.0000
  0.0000
effort [0]: []
time_from_start:
  sec: 0
  nanosec: 0
positions [1] 
  2.6253
  4.3878
  3.1693
  2.4180
  2.8000
velocities [1]: 
  -1.6000
  -1.1453
  0.0086
  1.2215
  -0.0000
accelerations [1]: 
  -16.0000
  -11.4525
  0.0865
  12.2151
  -0.0000
effort [1]: []
time_from_start:
  sec: 0
  nanosec: 100000000
positions [2] 
  2.3853
  4.2160
  3.1706
  2.6012
  2.8000
velocities [2]: 
  -3.2000
  -2.2905
  0.0173
  2.4430
  -0.0000
accelerations [2]: 
  -16.0000
  -11.4525
  0.0865
  12.2151
  -0.0000
effort [2]: []
time_from_start:
  sec: 0
  nanosec: 200000000
positions [3] 
  2.0074
  3.9455
  3.1726
  2.8897
  2.8000
velocities [3]: 
  -3.7379
  -2.6755
  0.0202
  2.8537
  -0.0000
accelerations [3]: 
  16.0000
  11.4525
  -0.0865
  -12.2151
  0.0000
effort [3]: []
time_from_start:
  sec: 0
  nanosec: 300000000
positions [4] 
  1.7136
  3.7352
  3.1742
  3.1140
  2.8000
velocities [4]: 
  -2.1379
  -1.5302
  0.0116
  1.6321
  -0.0000
accelerations [4]: 
  16.0000
  11.4525
  -0.0865
  -12.2151
  0.0000
effort [4]: []
time_from_start:
  sec: 0
  nanosec: 400000000
positions [5] 
  1.5798
  3.6395
  3.1750
  3.2161
  2.8000
velocities [5]: 
  -0.5379
  -0.3850
  0.0029
  0.4106
  -0.0000
accelerations [5]: 
  16.0000
  11.4525
  -0.0865
  -12.2151
  0.0000
effort [5]: []
time_from_start:
  sec: 0
  nanosec: 500000000
positions [6] 
  1.5708                                                                   
  3.6330                                                                   
  3.1750                                                                   
  3.2230                                                                   
  2.8000
velocities [6]: 
  -0.0000
  -0.0000
  0.0000
  0.0000
  -0.0000
accelerations [6]: 
  16.0000
  11.4525
  -0.0865
  -12.2151
  0.0000
effort [6]: []
time_from_start:
  sec: 0
  nanosec: 533615874

if we could set run time by calculating it from velocities and acceleration using the formula:
$t = \dfrac{\omega-\omega_0}{\alpha}$ where $\omega$ = final angular velocity, $\omega_0$ = initial angular velocity, $\alpha$ = angular acceleration, $t$ = run time.

Applying run times and corresponding positions as a block would be better. Also, we should take care of the joint whose position is not changing in the trajectory at every point in time. The current setPosition at the main branch is not suitable for this. I guess we need to brainstorm a bit to decide on an API that should be flexible.
Maybe similar to:

//              motor id       degrees                         milliseconds
Braccio.move({2, 3, 4, 5}).to({230.0, 90.0, 160.0, 160.0}).in({100, 80, 100, 90});

Or, using struct:

typedef struct {
  std::vector<int> id_list;
  std::vector<float> angles;
  std::vector<int> run_time;
} position_t;

position_t target_position; // init with values
Braccio.moveTo(target_position);

[metanav]

To keep the existing API, there is another way by overloading the setPosition and moveTo methods:

void SmartServoClass::setPosition(uint8_t const id, float const angle_deg, uint16_t const runtime);
void BraccioClass::moveTo(float *positions, uint16_t *runtimes);

[aentinger]

I'm open to discuss this. Maybe you can prepare an PR with a possible API?

[metanav]

One thing I wanted to clarify with the Smart Servo TX packet structure. Do we always need to send all 6 joint positions or can we send them selectively?

Arduino_Braccio_plusplus/src/lib/motors/SmartServo.cpp

Lines 258 to 276 in 3c9371e

	void SmartServoClass::synchronize()
	{
	mbed::ScopedLock<rtos::Mutex> lock(_mtx);
	_txPacket.id = 0xFE;
	_txPacket.length = MAX_TX_PAYLOAD_LEN;
	_txPacket.instruction = CMD(SmartServoOperation::SYNC_WRITE);
	_txPacket.payload[0] = REG(SmartServoRegister::TARGET_POSITION_H);
	_txPacket.payload[1] = 4;
	int index = 2;
	for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++) {
	_txPacket.payload[index++] = i;
	_txPacket.payload[index++] = _targetPosition[idToArrayIndex(i)]>>8;
	_txPacket.payload[index++] = _targetPosition[idToArrayIndex(i)];
	_txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)]>>8;
	_txPacket.payload[index++] = _targetSpeed[idToArrayIndex(i)];
	}
	sendPacket();
	}

I could not find any datasheet online for the SR312 or SR418D servos. In my code, I am handling the gripper and "base to wrist-roll" kinematic chain separately. The moveTo API is not flexible so I wanted to create a new one.

[aentinger]

One thing I wanted to clarify with the Smart Servo TX packet structure. Do we always need to send all 6 joint positions or can we send them selectively?

If you want all joints to act upon a new set-point at the same point in time then you need to send all six positions at once. Alternately you can update the servos individually but they will start moving towards the set-point as soon as their target position is updated.

There are no datasheets besides the motor specs listed here.