AS: creating final node
This commit is contained in:
@@ -29,8 +29,10 @@ class ScaledJointTrajectoryPublisher(Node):
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
print(args)
|
||||
if len(args) == len(self.joint_positions):
|
||||
self.joint_positions = args
|
||||
self.joint_positions = list(args)
|
||||
print(self.joint_positions)
|
||||
self.send_trajectory(self.joint_positions)
|
||||
elif len(args) == len(self.joint_positions) + 1:
|
||||
self.joint_positions = args[:-1]
|
||||
@@ -39,11 +41,12 @@ class ScaledJointTrajectoryPublisher(Node):
|
||||
|
||||
|
||||
|
||||
def send_trajectory(self, joint_positions, duration=3.0):
|
||||
def send_trajectory(self, joint_positions, duration=0.01):
|
||||
"""Publish a joint trajectory command to move the robot."""
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
point = JointTrajectoryPoint()
|
||||
joint_positions = [float(joint) for joint in joint_positions]
|
||||
point.positions = joint_positions # Updated joint positions
|
||||
point.time_from_start.sec = int(duration)
|
||||
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
|
||||
@@ -55,8 +58,8 @@ class ScaledJointTrajectoryPublisher(Node):
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
|
||||
tree = ET.parse('/BA/robot.urdf')
|
||||
robot_urdf = input("Enter the path to the URDF file: ")
|
||||
tree = ET.parse(robot_urdf)
|
||||
root = tree.getroot()
|
||||
joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
|
||||
|
||||
|
||||
296
workspace/build/joint_control/build/lib/joint_control/sandbox.py
Normal file
296
workspace/build/joint_control/build/lib/joint_control/sandbox.py
Normal file
@@ -0,0 +1,296 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from sensor_msgs.msg import JointState
|
||||
from osc4py3.as_allthreads import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
import numpy as np
|
||||
import spatialmath as sm
|
||||
import roboticstoolbox as rtb
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
|
||||
def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
1
|
||||
)
|
||||
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Increased queue size for joint states
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.current_joint_positions = [0.0] * len(joint_names)
|
||||
self.joint_names = joint_names
|
||||
self.joint_velocity_limits = joint_velocity_limits
|
||||
self.desired_joint_positions = [0.0] * len(joint_names)
|
||||
self.previous_desired = [0.0] * len(joint_names)
|
||||
self.robot = robot
|
||||
self.cost_mask = cost_mask
|
||||
self.prev_pose = None
|
||||
|
||||
ip = "0.0.0.0" # Listen on all network interfaces
|
||||
port = 8000 # Must match the sender's port in `joint_state_osc.py`
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server(ip, port, "osc_server")
|
||||
print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
|
||||
# Register OSC handler
|
||||
osc_method("/coordinates", self.coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
n=2
|
||||
for arg in args:
|
||||
if len(arg) == len(self.joint_names):
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(arg)
|
||||
point.time_from_start.sec = n
|
||||
n+=2
|
||||
point.time_from_start.nanosec = 0
|
||||
msg.points.append(point)
|
||||
elif len(arg) == len(self.joint_names) + 1:
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(arg[:-1])
|
||||
point.time_from_start.sec = int(arg[-1])
|
||||
point.time_from_start.nanosec = int((arg[-1] - int(arg[-1])) * 1e9)
|
||||
msg.points.append(point)
|
||||
|
||||
self.publisher.publish(msg)
|
||||
print("published joint positions")
|
||||
|
||||
set_limits = input("Do you want to set limit for x, y and z? (y/n): ").strip().lower()
|
||||
if set_limits == 'y':
|
||||
while True:
|
||||
try:
|
||||
self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
|
||||
self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
|
||||
self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
|
||||
|
||||
if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
|
||||
print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
|
||||
continue
|
||||
|
||||
if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
|
||||
(self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
|
||||
(self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
|
||||
print("Invalid input. Lower limit must be less than upper limit for each axis.")
|
||||
continue
|
||||
|
||||
print(f"Current limits:")
|
||||
print(f"x: {self.x_limits}")
|
||||
print(f"y: {self.y_limits}")
|
||||
print(f"z: {self.z_limits}")
|
||||
con = True
|
||||
while con:
|
||||
confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
|
||||
if confirm == 'y':
|
||||
break
|
||||
elif confirm == 'n':
|
||||
print("Please re-enter the limits.")
|
||||
con = False
|
||||
else:
|
||||
print("Invalid input. Please enter 'y' or 'n'.")
|
||||
if con: break
|
||||
except ValueError:
|
||||
print("Invalid input. Please enter numeric values only.")
|
||||
|
||||
# Ask the user if they want to set new joint limits
|
||||
update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
|
||||
if update_limits == 'y':
|
||||
for i in range(len(self.joint_names)):
|
||||
while True:
|
||||
try:
|
||||
lim = self.robot.qlim.copy()
|
||||
# Find the link corresponding to the joint name
|
||||
print(f"Current position limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
|
||||
lower_limit = input(f"Enter the new lower limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
|
||||
upper_limit = input(f"Enter the new upper limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
|
||||
|
||||
if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
|
||||
print("Invalid input. Lower limit must be less than upper limit.")
|
||||
continue
|
||||
|
||||
if lower_limit:
|
||||
lim[0][i] = float(lower_limit)
|
||||
if upper_limit:
|
||||
lim[1][i] = float(upper_limit)
|
||||
self.robot.qlim = lim
|
||||
print(f"New limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
|
||||
print("-" * 50)
|
||||
break
|
||||
except ValueError:
|
||||
print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
|
||||
'''
|
||||
use_link_mask = input("Do you want to use a link mask? (y/n): ").strip().lower()
|
||||
if use_link_mask == 'y':
|
||||
while True:
|
||||
try:
|
||||
'''
|
||||
|
||||
|
||||
self.hz = float(input("Enter the desired refresh frequency (Hz): "))
|
||||
# Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
|
||||
self.create_timer(1/self.hz, self.update_position) # Timer to call osc_process periodically
|
||||
|
||||
def coordinates_handler(self, *args):
|
||||
# Ensure the desired joint positions are within the specified limits
|
||||
#print("received joint angles")
|
||||
|
||||
x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
|
||||
if self.x_limits[0] is not None:
|
||||
x = max(self.x_limits[0], x)
|
||||
if self.x_limits[1] is not None:
|
||||
x = min(self.x_limits[1], x)
|
||||
if self.y_limits[0] is not None:
|
||||
y = max(self.y_limits[0], y)
|
||||
if self.y_limits[1] is not None:
|
||||
y = min(self.y_limits[1], y)
|
||||
if self.z_limits[0] is not None:
|
||||
z = max(self.z_limits[0], z)
|
||||
if self.z_limits[1] is not None:
|
||||
z = min(self.z_limits[1], z)
|
||||
|
||||
if x != args[0] or y != args[1] or z != args[2]:
|
||||
self.get_logger().warn(
|
||||
f"Desired joint positions adjusted to fit within limits: "
|
||||
f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
|
||||
)
|
||||
|
||||
self.desired_joint_positions = [x, y, z, r, p, yaw]
|
||||
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
joint_position_dict = dict(zip(msg.name, msg.position))
|
||||
self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
|
||||
joint_position_dict = dict(zip(msg.name, msg.velocity))
|
||||
self.current_joint_velocities = [joint_position_dict[name] for name in self.joint_names]
|
||||
|
||||
def update_position(self):
|
||||
if self.desired_joint_positions == self.previous_desired:
|
||||
return
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
steps_per_m = 100
|
||||
if True: #len(args[0]) == len(self.joint_names):
|
||||
prev_duration = 0
|
||||
if self.prev_pose == None:
|
||||
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
|
||||
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
|
||||
else:
|
||||
[x,y,z] = self.prev_pose[:3]
|
||||
[roll, pitch, yaw] = self.prev_pose[3:]
|
||||
x1, y1, z1, roll1, pitch1, yaw1 = self.desired_joint_positions
|
||||
self.prev_pose = self.desired_joint_positions
|
||||
steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
|
||||
if steps < 2: steps = 2
|
||||
cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
|
||||
for j in range(steps):
|
||||
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
|
||||
if sol[1] == 1:
|
||||
fowards = self.robot.fkine_all(sol[0])
|
||||
out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
|
||||
if np.any(out_of_bounds):
|
||||
#print(fowards.t)
|
||||
#indices = np.where(out_of_bounds)[0]
|
||||
#print(f"indices: {indices}")
|
||||
self.get_logger().warn("One or more links moved out of bounds!")
|
||||
'''
|
||||
for i in indices:
|
||||
try:
|
||||
print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
|
||||
except IndexError:
|
||||
print(f"index {i} is out of bounds, but no corresponding joint found.")
|
||||
self.previous_desired = self.desired_joint_positions
|
||||
'''
|
||||
break
|
||||
duration = 0
|
||||
prev = self.current_joint_positions if j == 0 else prev_sol
|
||||
for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
|
||||
duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
|
||||
prev_sol = list(sol[0])
|
||||
if duration == 0:
|
||||
continue
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(sol[0])
|
||||
duration *= 5
|
||||
duration += prev_duration
|
||||
prev_duration = duration
|
||||
point.time_from_start.sec = int(duration)
|
||||
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
|
||||
msg.points.append(point)
|
||||
else:
|
||||
print(f'IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!')
|
||||
prev_sol = self.current_joint_positions
|
||||
if len(msg.points) == 0:
|
||||
return
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
self.publisher.publish(msg)
|
||||
self.previous_desired = self.desired_joint_positions
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
robot_urdf = input("Enter the path to the URDF file: ")
|
||||
tree = ET.parse(robot_urdf)
|
||||
root = tree.getroot()
|
||||
joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
|
||||
robot = rtb.ERobot.URDF(robot_urdf)
|
||||
joint_velocity_limits = {}
|
||||
|
||||
# Iterate over all joints in the URDF
|
||||
for joint in root.findall('.//joint'):
|
||||
joint_name = joint.get('name') # Get the name of the joint
|
||||
|
||||
# Look for the <limit> tag under each joint
|
||||
limit = joint.find('limit')
|
||||
|
||||
if limit is not None:
|
||||
# Extract the velocity limit (if it exists)
|
||||
velocity_limit = limit.get('velocity')
|
||||
|
||||
if velocity_limit is not None:
|
||||
joint_velocity_limits[joint_name] = float(velocity_limit)
|
||||
|
||||
while True:
|
||||
try:
|
||||
print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
|
||||
print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
|
||||
cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
|
||||
if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
|
||||
break
|
||||
else:
|
||||
print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
|
||||
except ValueError:
|
||||
print("Invalid input. Please enter integers only.")
|
||||
print(f"Cost mask: {cost_mask}")
|
||||
|
||||
rclpy.init()
|
||||
|
||||
node = ScaledJointTrajectoryPublisher(joint_names, joint_velocity_limits, robot, cost_mask)
|
||||
|
||||
# Run ROS 2 spin, and osc_process will be handled by the timer
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -11,24 +11,31 @@ import time
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
def __init__(self, joint_names, robot):
|
||||
def __init__(self, joint_names, robot, cost_mask):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
self.cost_mask = cost_mask
|
||||
self.robot = robot
|
||||
self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
|
||||
|
||||
if self.trajectroy_topic_name == "":
|
||||
self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
self.trajectroy_topic_name,
|
||||
10
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.joint_names = joint_names
|
||||
|
||||
self.port = 8000 # UDP port
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", 6080, "osc_server")
|
||||
print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
|
||||
osc_udp_server("0.0.0.0", self.port, "osc_server")
|
||||
print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
@@ -41,26 +48,53 @@ class ScaledJointTrajectoryPublisher(Node):
|
||||
joint_positions = [0.0] * len(self.joint_names)
|
||||
steps = 30
|
||||
vel = 0.4
|
||||
if len(args[0]) == len(self.joint_names):
|
||||
if True: #len(args[0]) == len(self.joint_names):
|
||||
n=2.0
|
||||
for i in range(len(args)-1):
|
||||
print(f'i = {i}')
|
||||
x, y, z, roll, pitch, yaw = args[i]
|
||||
print(1)
|
||||
Tep1 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
|
||||
print(2)
|
||||
x, y, z, roll, pitch, yaw = args[i+1]
|
||||
print(3)
|
||||
Tep2 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
|
||||
print(4)
|
||||
cart_traj = rtb.ctraj(Tep1, Tep2, steps)
|
||||
for j in range(steps-1):
|
||||
sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions)
|
||||
dist = np.linalg.norm(cart_traj[j].t - cart_traj[j+1].t)
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(sol[0])
|
||||
joint_positions = list(sol[0])
|
||||
point.time_from_start.sec = int(n)
|
||||
point.time_from_start.nanosec = int((n - int(n)) * 1e9)
|
||||
n+=dist/vel
|
||||
n+=0.1
|
||||
msg.points.append(point)
|
||||
print(cart_traj)
|
||||
print(5)
|
||||
for j in range(steps):
|
||||
print(f'j = {j}')
|
||||
print(6)
|
||||
sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions, mask = self.cost_mask, joint_limits = True)
|
||||
print(7)
|
||||
if sol[1] == 1:
|
||||
print(8)
|
||||
if j == 0: dist = vel*n
|
||||
else: dist = np.linalg.norm(cart_traj[j].t - cart_traj[j-1].t)
|
||||
print(9)
|
||||
point = JointTrajectoryPoint()
|
||||
print(10)
|
||||
point.positions = list(sol[0])
|
||||
print(11)
|
||||
joint_positions = list(sol[0])
|
||||
print(12)
|
||||
point.time_from_start.sec = int(n)
|
||||
print(13)
|
||||
point.time_from_start.nanosec = int((n - int(n)) * 1e9)
|
||||
print(14)
|
||||
n+=dist/vel
|
||||
print(16)
|
||||
msg.points.append(point)
|
||||
print(17)
|
||||
else: print('IK could not find a solution!')
|
||||
print(18)
|
||||
self.publisher.publish(msg)
|
||||
print(19)
|
||||
print(f"published joint positions {msg.points[-1]}")
|
||||
print(f'Frequency: {round(1/(time.time()-time1),2)} Hz')
|
||||
|
||||
'''
|
||||
elif len(args[0]) == len(self.joint_names) + 1:
|
||||
for i in range(len(args)):
|
||||
x, y, z, roll, pitch, yaw, timetag = args[i]
|
||||
@@ -72,23 +106,32 @@ class ScaledJointTrajectoryPublisher(Node):
|
||||
sol = self.robot.ik_LM(Tep, q0=joint_positions)
|
||||
|
||||
else:
|
||||
print("Invalid number or format of arguments")
|
||||
|
||||
self.publisher.publish(msg)
|
||||
print("published joint positions")
|
||||
print(f'Frequency: {round(1/(time.time()-time1),2)} Hz')
|
||||
print("Invalid number or format of arguments")'''
|
||||
|
||||
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
|
||||
tree = ET.parse('/BA/robot.urdf')
|
||||
path_to_urdf = input("Enter the path to the URDF file: ")
|
||||
tree = ET.parse(path_to_urdf)
|
||||
root = tree.getroot()
|
||||
joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
|
||||
robot = rtb.ERobot.URDF('/BA/robot.urdf')
|
||||
robot = rtb.ERobot.URDF(path_to_urdf)
|
||||
print(robot)
|
||||
rclpy.init()
|
||||
|
||||
node = ScaledJointTrajectoryPublisher(joint_names, robot)
|
||||
while True:
|
||||
try:
|
||||
print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
|
||||
print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
|
||||
cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0) separated by spaces, of which <= {robot.n} are 1): ").split()]
|
||||
if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
|
||||
break
|
||||
else:
|
||||
print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
|
||||
except ValueError:
|
||||
print("Invalid input. Please enter integers only.")
|
||||
print(f"Cost mask: {cost_mask}")
|
||||
node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask)
|
||||
|
||||
# Run both ROS 2 and OSC Server together
|
||||
try:
|
||||
|
||||
@@ -1,15 +1,16 @@
|
||||
AMENT_PREFIX_PATH=/opt/ros/humble
|
||||
AMENT_PREFIX_PATH=/BA/workspace/install/painting_robot_control:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble
|
||||
COLCON=1
|
||||
COLCON_PREFIX_PATH=/BA/workspace/install
|
||||
HOME=/root
|
||||
HOSTNAME=hapticslab2
|
||||
HOSTNAME=0e38e264ac6b
|
||||
LANG=C.UTF-8
|
||||
LC_ALL=C.UTF-8
|
||||
LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib
|
||||
LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
|
||||
OLDPWD=/ws/src/ba-alexanderschaefer
|
||||
OLDPWD=/BA
|
||||
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
|
||||
PWD=/ws/src/ba-alexanderschaefer/workspace/build/joint_control
|
||||
PYTHONPATH=/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
|
||||
PWD=/BA/workspace/build/joint_control
|
||||
PYTHONPATH=/BA/workspace/build/painting_robot_control:/BA/workspace/install/painting_robot_control/lib/python3.10/site-packages:/BA/workspace/build/mock_robot:/BA/workspace/install/mock_robot/lib/python3.10/site-packages:/BA/workspace/build/joint_info:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
|
||||
ROS_DISTRO=humble
|
||||
ROS_LOCALHOST_ONLY=0
|
||||
ROS_PYTHON_VERSION=3
|
||||
|
||||
@@ -15,6 +15,7 @@ joint_control/plugdata.py
|
||||
joint_control/plugdata_cart.py
|
||||
joint_control/plugdata_cart_fix.py
|
||||
joint_control/plugdata_cart_smooth.py
|
||||
joint_control/sandbox.py
|
||||
joint_control/test.py
|
||||
joint_control/trajectory_server.py
|
||||
joint_control/trajectory_server_cart.py
|
||||
|
||||
@@ -5,6 +5,7 @@ plugdata = joint_control.plugdata:main
|
||||
plugdata_cart = joint_control.plugdata_cart:main
|
||||
plugdata_cart_fix = joint_control.plugdata_cart_fix:main
|
||||
plugdata_cart_smooth = joint_control.plugdata_cart_smooth:main
|
||||
sandbox = sandbox.sandbox:main
|
||||
test = joint_control.test:main
|
||||
trajectory_server = joint_control.trajectory_server:main
|
||||
trajectory_server_cart = joint_control.trajectory_server_cart:main
|
||||
|
||||
Binary file not shown.
@@ -1,4 +1,4 @@
|
||||
import sys
|
||||
if sys.prefix == '/usr':
|
||||
sys.real_prefix = sys.prefix
|
||||
sys.prefix = sys.exec_prefix = '/ws/src/ba-alexanderschaefer/workspace/install/joint_control'
|
||||
sys.prefix = sys.exec_prefix = '/BA/workspace/install/joint_control'
|
||||
|
||||
Reference in New Issue
Block a user