Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Cartesian velocity control #191

Closed
wants to merge 3 commits into from
Closed
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
521 changes: 521 additions & 0 deletions lbr_demos/lbr_demos_py/lbr_demos_py/asbr.py

Large diffs are not rendered by default.

5 changes: 5 additions & 0 deletions lbr_demos/lbr_demos_py/lbr_demos_py/joint_sine_overlay.py
Original file line number Diff line number Diff line change
Expand Up @@ -49,13 +49,18 @@ def _on_lbr_state(self, lbr_state: LBRState) -> None:
self._lbr_joint_position_command.joint_position[
3
] += self._amplitude * math.sin(self._phase)

self._lbr_joint_position_command.joint_position[
5
] += -self._amplitude * math.sin(self._phase)
self._phase += 2 * math.pi * self._frequency * self._dt

self._lbr_joint_position_command_pub.publish(
self._lbr_joint_position_command
)
else:
# reset phase
print('resetting')
self._phase = 0.0

def _retrieve_update_rate(self) -> float:
Expand Down
208 changes: 208 additions & 0 deletions lbr_demos/lbr_demos_py/lbr_demos_py/move_2_cart.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,208 @@
#! /usr/bin/env python3

import rclpy
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

you'll have to run the black formatter to pass the linting tests for all python files

from rclpy.node import Node
from geometry_msgs.msg import Pose
import math
import copy
import time
import numpy as np
from lbr_demos_py.asbr import *

class Move2Cart(Node):

def __init__(self):
super().__init__('move_2_cart')
self.is_init = False
self.curr_pose = Pose()
self.goal_pose = Pose() # get from constructor?

self.communication_rate = 0.01 # 10 ms
self.pid_p_correction = 12.2 # For compensating delay during execution. This value -in accordance with p = 12.2 - was found to be good by manual testings.
# See: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/187

self.move_orientation_init_time = -1

self.pose_pub = self.create_publisher(Pose, 'command/pose', 1)
self.pose_sub = self.create_subscription(Pose, 'state/pose', self.on_pose, 1)
self.flag=1


def on_pose(self, msg):

if not self.is_init:
self.is_init = True
self.initial_pose = msg

#Specify goal pose

self.goal_pose.position.x = 0.5
self.goal_pose.position.y = 0.0
self.goal_pose.position.z = 0.55

goal_orientation = Rotation(self.initial_pose.orientation)
goal_orientation = goal_orientation.as_ABC(True)
goal_orientation[1]=10.0
goal_orientation[2]=-160.0
goal_orientation[0]=180.0
goal_orientation = Rotation.from_ABC(goal_orientation,True)
goal_orientation = goal_orientation.as_geometry_orientation()
self.goal_pose.orientation = goal_orientation

self.desired_pose = msg # desired_pose: the pose which the robot should be in, at the moment. This is useful particularly in orientation control.
# For some reason, the orientation gets some noise during execution. Therefore, relying on curr_pose.orientation for generating
# commands was giving problems.

print('starting at:', int(round(time.time() * 1000))) # Current time in milliseconds


else:

self.curr_pose = msg

if (self.is_close_pos() and self.is_close_orien()):

if(self.flag==1):
current_time_ms = int(round(time.time() * 1000)) # Current time in milliseconds
print(f'Time (ms): {current_time_ms}')
self.flag=0
return
elif (self.is_close_pos()):
MotionTime = 20.0 #s
command_pose = self.generate_move_command_rotation(MotionTime)
# print('here')
else:
lin_vel = 0.01 # replace the number with desired linear velocity in m/s
command_pose = self.generate_move_command(lin_vel)
# print('I AM HERE!')


if (command_pose.position.x > 0.75 or command_pose.position.x < 0.35 or
command_pose.position.y > 0.1 or command_pose.position.y < -0.1 or
command_pose.position.z > 0.78 or command_pose.position.z < 0.5): #Command guarding

print('Failed, not executable')

else:
self.pose_pub.publish(command_pose)


def generate_move_command(self, lin_vel, pos_thresh = 0.0005): #Generates move commands, for motions containing ONLY rotational movements use generate_move_command_rotation method
command_pose = Pose()
GoalPose = self.goal_pose
CurrPose = self.curr_pose

translation_vec = np.asarray([GoalPose.position.x - CurrPose.position.x,
GoalPose.position.y - CurrPose.position.y,
GoalPose.position.z - CurrPose.position.z])
if(np.linalg.norm(translation_vec)<2*pos_thresh):
vel_vec = (translation_vec / np.linalg.norm(translation_vec)) * lin_vel
else:
vel_vec = (translation_vec / np.linalg.norm(translation_vec)) * lin_vel * self.pid_p_correction
command_pose.position.x = CurrPose.position.x + vel_vec[0] * self.communication_rate
command_pose.position.y = CurrPose.position.y + vel_vec[1] * self.communication_rate
command_pose.position.z = CurrPose.position.z + vel_vec[2] * self.communication_rate


timeRemain2Reach = np.linalg.norm(translation_vec) / lin_vel
if(int(timeRemain2Reach/self.communication_rate)>0):
curr_Rot = Rotation(CurrPose.orientation)
goal_Rot = Rotation(GoalPose.orientation)
ABC_diff = goal_Rot.as_ABC() - (Rotation(self.desired_pose.orientation)).as_ABC()
for i in range(3):
if(ABC_diff[i]>np.pi):
ABC_diff[i]-=2.0*np.pi
elif(ABC_diff[i]<(-np.pi)):
ABC_diff[i]+=2.0*np.pi

ABC_step = (ABC_diff / timeRemain2Reach) * self.communication_rate
ABC_command = ABC_step + (Rotation(self.desired_pose.orientation)).as_ABC()
quat_command = Rotation.from_ABC(ABC_command)

command_pose.orientation = quat_command.as_geometry_orientation()
self.desired_pose = copy.deepcopy(command_pose)
self.last_command = command_pose
else:
command_pose.orientation = self.last_command.orientation
# command_pose.orientation = self.initial_pose.orientation


return command_pose

def generate_move_command_rotation(self, motion_time, angle_thresh = 0.1*3.1415/180.0):
GoalPose = self.goal_pose
CurrPose = self.curr_pose
command_pose = copy.deepcopy(CurrPose)

if(self.move_orientation_init_time == -1):
self.move_orientation_init_time = int(round(time.time() * 1000))/1000.0 # Current time in seconds
else:
curr_time = int(round(time.time() * 1000))/1000.0 # Current time in seconds
timeRemain2Reach = motion_time - (curr_time - self.move_orientation_init_time)
if(int(timeRemain2Reach/self.communication_rate)>0):
curr_Rot = Rotation(CurrPose.orientation)
goal_Rot = Rotation(GoalPose.orientation)
ABC_diff = goal_Rot.as_ABC() - (Rotation(self.desired_pose.orientation)).as_ABC()
for i in range(3):
if(ABC_diff[i]>np.pi):
ABC_diff[i]-=2.0*np.pi
elif(ABC_diff[i]<(-np.pi)):
ABC_diff[i]+=2.0*np.pi

ABC_step = (ABC_diff / timeRemain2Reach) * self.communication_rate
ABC_command = ABC_step + (Rotation(self.desired_pose.orientation)).as_ABC()
quat_command = Rotation.from_ABC(ABC_command)

command_pose.orientation = quat_command.as_geometry_orientation()


# if(np.max(np.abs(ABC_command - curr_Rot.as_ABC()))>0.2*3.14159/180.0):
# if(int(round(time.time() * 1000))%5==0):
# print(ABC_command)
# print(curr_Rot.as_ABC())
# print((Rotation(self.desired_pose.orientation)).as_ABC())
# print('Too much jump')
# return False

self.last_command = command_pose
self.desired_pose = copy.deepcopy(command_pose)

else:
command_pose.orientation = self.last_command.orientation


return command_pose



def is_close_pos(self, pos_thresh = 0.0005):
translation_vec = np.asarray([self.goal_pose.position.x - self.curr_pose.position.x,
self.goal_pose.position.y - self.curr_pose.position.y,
self.goal_pose.position.z - self.curr_pose.position.z])

return np.linalg.norm(translation_vec)<pos_thresh

def is_close_orien(self, angle_thresh = 0.1*3.1415/180.0):
curr_Rot = Rotation(self.curr_pose.orientation)
goal_Rot = Rotation(self.goal_pose.orientation)
ABC_diff = curr_Rot.as_ABC() - goal_Rot.as_ABC()
for i in range(3):
if(ABC_diff[i]>np.pi):
ABC_diff[i]-=2.0*np.pi
elif(ABC_diff[i]<(-np.pi)):
ABC_diff[i]+=2.0*np.pi
if(np.max(np.abs(ABC_diff))<angle_thresh):
self.move_orientation_init_time = -1

return np.max(np.abs(ABC_diff))<angle_thresh


def main(args=None):
rclpy.init(args=args)
node = Move2Cart()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()

if __name__ == '__main__':
main()
47 changes: 47 additions & 0 deletions lbr_demos/lbr_demos_py/lbr_demos_py/pose_planning.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose
import math

class PosePlanningNode(Node):
def __init__(self):
super().__init__('pose_planning')
self.is_init = False
self.amplitude = 0.05
self.frequency = 0.2
self.sampling_time = 0.01
self.phase = 0.0

self.pose_pub = self.create_publisher(Pose, 'command/pose', 1)
self.pose_sub = self.create_subscription(Pose, 'state/pose', self.on_pose, 1)

def on_pose(self, msg):
if not self.is_init:
self.initial_pose = msg
self.is_init = True
else:

cartesian_pose_command = Pose()
cartesian_pose_command.position.x = self.initial_pose.position.x
cartesian_pose_command.position.y = self.initial_pose.position.y
cartesian_pose_command.position.z = self.initial_pose.position.z
print('...')
print(msg.position.z)

self.phase += 2 * math.pi * self.frequency * self.sampling_time
cartesian_pose_command.position.z += self.amplitude * math.sin(self.phase)

cartesian_pose_command.orientation = self.initial_pose.orientation

self.pose_pub.publish(cartesian_pose_command)
print(cartesian_pose_command.position.z)

def main(args=None):
rclpy.init(args=args)
node = PosePlanningNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()

if __name__ == '__main__':
main()
2 changes: 2 additions & 0 deletions lbr_demos/lbr_demos_py/setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,8 @@
"joint_trajectory_client = lbr_demos_py.joint_trajectory_client:main",
"torque_sine_overlay = lbr_demos_py.torque_sine_overlay:main",
"wrench_sine_overlay = lbr_demos_py.wrench_sine_overlay:main",
"pose_planning = lbr_demos_py.pose_planning:main",
"move_2_cart = lbr_demos_py.move_2_cart:main",
],
},
)
10 changes: 4 additions & 6 deletions lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -126,20 +126,18 @@ class JointPIDArray {
using pid_array_t = std::array<control_toolbox::Pid, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;

public:
JointPIDArray() = default;
JointPIDArray() = delete;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

could you please make sure to pull the latest changes from the humble branch?

So that no changes are made to the driver.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks @mhubii. I'm a little confused. Didn't you change the pid parameters and how they are configured based on the Issue in this commit?
I was checking my lbr_system_parameters.yaml with yours and they're the same?

JointPIDArray(const PIDParameters &pid_parameters);

void compute(const value_array_t &command_target, const value_array_t &state,
const std::chrono::nanoseconds &dt, value_array_t &command);
void compute(const value_array_t &command_target, const double *state,
const std::chrono::nanoseconds &dt, value_array_t &command);
void initialize(const PIDParameters &pid_parameters, const double &dt);
inline const bool &is_initialized() const { return initialized_; };

void log_info() const;

protected:
bool initialized_{false}; /**< True if initialized.*/
pid_array_t pid_controllers_; /**< PID controllers for each joint.*/
PIDParameters pid_parameters_; /**< PID parameters for all joints.*/
pid_array_t pid_controllers_; /**< PID controllers for each joint.*/
};
} // end of namespace lbr_fri_ros2
#endif // LBR_FRI_ROS2__FILTERS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,6 @@ class BaseCommandInterface {

protected:
std::unique_ptr<CommandGuard> command_guard_;
PIDParameters pid_parameters_;
JointPIDArray joint_position_pid_;
idl_command_t command_, command_target_;
};
Expand Down
28 changes: 21 additions & 7 deletions lbr_fri_ros2/src/filters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,17 @@ void JointExponentialFilterArray::initialize(const double &cutoff_frequency,
initialized_ = true;
}

JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters)
: pid_parameters_(pid_parameters) // keep local copy of parameters since
// controller_toolbox::Pid::getGains is not const correct
// (i.e. can't be called in this->log_info)
{
std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) {
pid.initPid(pid_parameters_.p, pid_parameters_.i, pid_parameters_.d, pid_parameters_.i_max,
pid_parameters_.i_min, pid_parameters_.antiwindup);
});
}

void JointPIDArray::compute(const value_array_t &command_target, const value_array_t &state,
const std::chrono::nanoseconds &dt, value_array_t &command) {
std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable {
Expand All @@ -59,11 +70,14 @@ void JointPIDArray::compute(const value_array_t &command_target, const double *s
});
}

void JointPIDArray::initialize(const PIDParameters &pid_parameters, const double &dt) {
std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) {
pid.initPid(pid_parameters.p * dt, pid_parameters.i * dt, pid_parameters.d * dt,
pid_parameters.i_max * dt, pid_parameters.i_min * dt, pid_parameters.antiwindup);
});
initialized_ = true;
}
void JointPIDArray::log_info() const {
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:");
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* p: %.1f", pid_parameters_.p);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i: %.1f", pid_parameters_.i);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* d: %.1f", pid_parameters_.d);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_max: %.1f", pid_parameters_.i_max);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_min: %.1f", pid_parameters_.i_min);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* antiwindup: %s",
pid_parameters_.antiwindup ? "true" : "false");
};
} // end of namespace lbr_fri_ros2
11 changes: 2 additions & 9 deletions lbr_fri_ros2/src/interfaces/base_command.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ namespace lbr_fri_ros2 {
BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters,
const CommandGuardParameters &command_guard_parameters,
const std::string &command_guard_variant)
: pid_parameters_(pid_parameters) {
: joint_position_pid_(pid_parameters) {
command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant);
};

Expand All @@ -18,13 +18,6 @@ void BaseCommandInterface::init_command(const_idl_state_t_ref state) {

void BaseCommandInterface::log_info() const {
command_guard_->log_info();
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*** Parameters:");
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.p: %.1f", pid_parameters_.p);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i: %.1f", pid_parameters_.i);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.d: %.1f", pid_parameters_.d);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i_max: %.1f", pid_parameters_.i_max);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i_min: %.1f", pid_parameters_.i_min);
RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.antiwindup: %s",
pid_parameters_.antiwindup ? "true" : "false");
joint_position_pid_.log_info();
}
} // namespace lbr_fri_ros2
Loading
Loading