diff --git a/README.md b/README.md
index 4328657..3fc1174 100644
--- a/README.md
+++ b/README.md
@@ -20,6 +20,7 @@ We will try to maintain compatibility with ROS Melodic for as long as possible,
## Robots currently implemented
- MiR100
- Universal Robots: UR3, UR3e, UR5, UR5e, UR10, UR10e, UR16
+- Franka Emika Panda Robot
diff --git a/panda_robot_server/config/panda_gripper_controller.yaml b/panda_robot_server/config/panda_gripper_controller.yaml
new file mode 100644
index 0000000..7f6380c
--- /dev/null
+++ b/panda_robot_server/config/panda_gripper_controller.yaml
@@ -0,0 +1,13 @@
+
+panda_gripper_controller:
+ type: panda_sim_controllers/PandaGripperController
+ pub_topic: /franka_gripper/joint_states
+ joints:
+ # main joint
+ panda_finger_joint1_controller:
+ joint: panda_finger_joint1
+ pid: {p: 5000, i: 50, d: 0.5}
+ # mimic joint
+ panda_finger_joint2_controller:
+ joint: panda_finger_joint2
+ pid: {p: 5000, i: 50, d: 0.5}
diff --git a/panda_robot_server/config/panda_sim_controllers.yaml b/panda_robot_server/config/panda_sim_controllers.yaml
new file mode 100644
index 0000000..9ddc303
--- /dev/null
+++ b/panda_robot_server/config/panda_sim_controllers.yaml
@@ -0,0 +1,101 @@
+panda_simulator:
+
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 100
+
+
+ # Panda SDK Controllers: Position --------------------------
+ position_joint_position_controller:
+ type: panda_sim_controllers/PandaPositionController
+ topic_joint_command: /panda_simulator/motion_controller/arm/joint_commands
+ topic_set_speed_ratio: /panda_simulator/arm/set_speed_ratio
+ joints:
+ panda_joint1_controller:
+ joint: panda_joint1
+ pid: {p: 3000, i: 10.0, d: 25.0}
+ panda_joint2_controller:
+ joint: panda_joint2
+ pid: {p: 3000, i: 10.0, d: 30.0}
+ panda_joint3_controller:
+ joint: panda_joint3
+ pid: {p: 3000, i: 10.0, d: 30.0}
+ panda_joint4_controller:
+ joint: panda_joint4
+ pid: {p: 3000, i: 10.0, d: 50.0}
+ panda_joint5_controller:
+ joint: panda_joint5
+ pid: {p: 3000, i: 10.0, d: 8.0}
+ panda_joint6_controller:
+ joint: panda_joint6
+ pid: {p: 3000, i: 1.0, d: 5.0}
+ panda_joint7_controller:
+ joint: panda_joint7
+ pid: {p: 3000, i: 10.0, d: 4.0}
+
+ # Panda SDK Controllers: Velocity --------------------------
+ velocity_joint_velocity_controller:
+ type: panda_sim_controllers/PandaVelocityController
+ topic: /panda_simulator/motion_controller/arm/joint_commands
+ joints:
+ panda_joint1_controller:
+ joint: panda_joint1
+ pid: {p: 10, i: 0.0, d: 0.1}
+ panda_joint2_controller:
+ joint: panda_joint2
+ pid: {p: 100, i: 1.0, d: 0.1}
+ panda_joint3_controller:
+ joint: panda_joint3
+ pid: {p: 0.05, i: 0.0, d: 0.01}
+ panda_joint4_controller:
+ joint: panda_joint4
+ pid: {p: 0.5, i: 0.01, d: 0.1}
+ panda_joint5_controller:
+ joint: panda_joint5
+ pid: {p: 1.0, i: 0.0, d: 0.01}
+ panda_joint6_controller:
+ joint: panda_joint6
+ pid: {p: 0.05, i: 0.0, d: 0.01}
+ panda_joint7_controller:
+ joint: panda_joint7
+ pid: {p: 0.05, i: 0.0, d: 0.01}
+
+ # Panda SDK Controllers: Effort --------------------------
+ effort_joint_torque_controller:
+ type: panda_sim_controllers/PandaEffortController
+ topic: /panda_simulator/motion_controller/arm/joint_commands
+ joints:
+ panda_joint1_controller:
+ joint: panda_joint1
+ panda_joint2_controller:
+ joint: panda_joint2
+ panda_joint3_controller:
+ joint: panda_joint3
+ panda_joint4_controller:
+ joint: panda_joint4
+ panda_joint5_controller:
+ joint: panda_joint5
+ panda_joint6_controller:
+ joint: panda_joint6
+ panda_joint7_controller:
+ joint: panda_joint7
+
+ # Panda SDK Controllers: Gravity Compensation ------------
+ effort_joint_gravity_controller:
+ type: panda_sim_controllers/PandaGravityController
+ joints:
+ panda_joint1_controller:
+ joint: panda_joint1
+ panda_joint2_controller:
+ joint: panda_joint2
+ panda_joint3_controller:
+ joint: panda_joint3
+ panda_joint4_controller:
+ joint: panda_joint4
+ panda_joint5_controller:
+ joint: panda_joint5
+ panda_joint6_controller:
+ joint: panda_joint6
+ panda_joint7_controller:
+ joint: panda_joint7
+
diff --git a/panda_robot_server/config/panda_sim_controllers_plugins.xml b/panda_robot_server/config/panda_sim_controllers_plugins.xml
new file mode 100644
index 0000000..e97b87c
--- /dev/null
+++ b/panda_robot_server/config/panda_sim_controllers_plugins.xml
@@ -0,0 +1,77 @@
+
+
+
+
+ The PandaGripperController tracks position commands for the electric grippers. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+
+
+
+
+
+ The PandaPositionController tracks position commands for the entire Panda arm. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+
+
+
+
+
+ The PandaPositionController tracks velocity commands for the entire Panda arm. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+
+
+
+
+
+ The PandaPositionController tracks effort commands for the entire Panda arm. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+
+
+
+
+
+ The PandaGravityController adds an effort component to compensate for gravity. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+
+
+
+
+
+
+ The JointPositionController tracks position commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+
+
+
+
+
+ The JointVelocityController tracks velocity commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+
+
+
+
+
+ The JointEffortController tracks effort commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+
+
+
+
+
+ The JointEffortController tracks effort commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+
+
+
+
+
diff --git a/panda_robot_server/launch/inc/load_panda_sim.launch b/panda_robot_server/launch/inc/load_panda_sim.launch
index cc3deff..47e58cd 100644
--- a/panda_robot_server/launch/inc/load_panda_sim.launch
+++ b/panda_robot_server/launch/inc/load_panda_sim.launch
@@ -53,7 +53,7 @@
-
+
@@ -84,4 +84,4 @@
-
\ No newline at end of file
+
diff --git a/panda_robot_server/launch/panda_robot_server.launch b/panda_robot_server/launch/panda_robot_server.launch
index 77e5f80..cc8ddaf 100644
--- a/panda_robot_server/launch/panda_robot_server.launch
+++ b/panda_robot_server/launch/panda_robot_server.launch
@@ -10,6 +10,7 @@
+
@@ -87,6 +88,7 @@
+
diff --git a/panda_robot_server/launch/real_panda_robot_server.launch b/panda_robot_server/launch/real_panda_robot_server.launch
new file mode 100644
index 0000000..ebaf4e0
--- /dev/null
+++ b/panda_robot_server/launch/real_panda_robot_server.launch
@@ -0,0 +1,98 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/panda_robot_server/scripts/robot_server.py b/panda_robot_server/scripts/robot_server.py
index 7b7d10d..fedbb14 100755
--- a/panda_robot_server/scripts/robot_server.py
+++ b/panda_robot_server/scripts/robot_server.py
@@ -28,7 +28,7 @@ def SetState(self, request, context):
def SendAction(self, request, context):
try:
- executed_action = self.rosbridge.publish_env_arm_cmd(request.action)
+ executed_action = self.rosbridge.send_action(request.action)
return robot_server_pb2.Success(success=1)
except:
rospy.logerr('Failed to send action', exc_info=True)
@@ -36,7 +36,7 @@ def SendAction(self, request, context):
def SendActionGetState(self, request, context):
try:
- executed_action = self.rosbridge.publish_env_arm_cmd(request.action)
+ executed_action = self.rosbridge.send_action(request.action)
return self.rosbridge.get_state()
except:
rospy.logerr('Failed to send action and get state', exc_info=True)
diff --git a/panda_robot_server/src/panda_robot_server/ros_bridge.py b/panda_robot_server/src/panda_robot_server/ros_bridge.py
index e577f38..7365cd6 100755
--- a/panda_robot_server/src/panda_robot_server/ros_bridge.py
+++ b/panda_robot_server/src/panda_robot_server/ros_bridge.py
@@ -1,16 +1,21 @@
#! /usr/bin/env python
+from ast import Break, While
+from configparser import RawConfigParser
+from tkinter import VERTICAL
+from cv2 import trace
import rospy
import tf2_ros
from gazebo_msgs.msg import ContactsState
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
-from std_msgs.msg import Bool
+from std_msgs.msg import Bool, Header
from franka_interface import ArmInterface
import copy
# See https://docs.python.org/3/library/threading.html#event-objects
from threading import Event
from robo_gym_server_modules.robot_server.grpc_msgs.python import robot_server_pb2
+import numpy as np
class PandaRosBridge:
@@ -35,12 +40,16 @@ def __init__(self, real_robot=False):
# Robot control
self.arm_cmd_pub = rospy.Publisher('env_arm_command', JointTrajectory, queue_size=1) # joint_trajectory_command_handler publisher
- self.sleep_time = (1.0 / rospy.get_param('~action_cycle_rate')) - 0.01
+ self.sleep_time = (1.0 / rospy.get_param('~action_cycle_rate')) # - 0.01
self.control_period = rospy.Duration.from_sec(self.sleep_time)
self.max_velocity_scale_factor = float(rospy.get_param("~max_velocity_scale_factor"))
self.min_traj_duration = 0.5 # minimum trajectory duration (s)
self.joint_velocity_limits = self._get_joint_velocity_limits()
-
+ self.velocity_limits = [2.175, 2.175, 2.175, 2.175, 2.61, 2.61, 2.61]
+ self.zero_vel= [0., 0., 0., 0., 0., 0., 0.]
+ self.zero_vel_cmd = self._transform_panda_list_to_dict(self.zero_vel[0:7])
+ self.velocity_limits = [x * self.max_velocity_scale_factor for x in self.velocity_limits]
+ self.robot_moving = False
# Robot frames
self.reference_frame = rospy.get_param('~reference_frame', 'base')
self.ee_frame = 'panda_hand'
@@ -68,9 +77,12 @@ def __init__(self, real_robot=False):
# Robot Server mode
self.rs_mode = rospy.get_param('~rs_mode')
+ # Action Mode
+ self.action_mode = rospy.get_param('~action_mode')
+
# Objects Controller
self.objects_controller = rospy.get_param("objects_controller", False)
- self.n_objects = int(rospy.get_param("n_objects"))
+ self.n_objects = int(rospy.get_param("n_objects", 0))
if self.objects_controller:
self.move_objects_pub = rospy.Publisher('move_objects', Bool, queue_size=10)
# Get objects model name
@@ -167,6 +179,14 @@ def get_state(self):
def set_state(self, state_msg):
# Set environment state
+ if self.real_robot: # When using real robot. the velocity message should not suddenly stop publishing.
+ for i in range(100):
+ self.arm.set_joint_velocities(self.zero_vel_cmd)
+ print(self.zero_vel_cmd)
+ rospy.sleep(self.control_period)
+
+
+ rospy.sleep(3.0) # Only use in velocity mode. when using high publish rate (400Hz)
state = state_msg.state
# Clear reset Event
@@ -187,10 +207,11 @@ def set_state(self, state_msg):
rospy.set_param(param, state_msg.float_params[param])
positions = self._get_joint_position_dict_from_rs_dict(state_msg.state_dict)
- reset_steps = int(15.0 / self.sleep_time)
+ #reset_steps = int(15.0 / self.sleep_time)
# for _ in range(reset_steps):
# self.arm.set_joint_positions(positions)
+ #self.arm.set_joint_positions_velocities(positions[0:7], [0.0]*7)
self.arm.move_to_joint_positions(positions, use_moveit=False)
if not self.real_robot:
@@ -208,7 +229,30 @@ def set_state(self, state_msg):
return True
+ def send_action(self, action): # adding action modes
+
+ if self.action_mode == 'abs_pos':
+ executed_action = self.publish_env_arm_cmd(action)
+
+ elif self.action_mode == 'delta_pos':
+ executed_action = self.publish_env_arm_delta_cmd(action)
+
+ elif self.action_mode == 'delta_vel':
+ executed_action = self.publish_env_arm_delta_vel_cmd(action)
+
+ return executed_action
+
+
+ #TODO For fixining the acceleration issue you should change the following function (for debugging: step1: tmux -L ServerManager //// step2: Crtl+B then '(' )
+
def publish_env_arm_cmd(self, position_cmd):
+
+ transformed_j_pos = self._transform_panda_list_to_dict(position_cmd[0:7])
+
+ #self.arm.set_joint_positions(transformed_j_pos)
+ self.arm.set_joint_velocities(transformed_j_pos)
+
+ rospy.sleep(self.control_period)
"""Publish environment JointTrajectory msg.
Publish JointTrajectory message to the env_command topic.
@@ -218,35 +262,67 @@ def publish_env_arm_cmd(self, position_cmd):
Returns:
type: Description of returned object.
+ """
+ def publish_env_arm_delta_cmd(self, delta_cmd): # Joint Position Controller difference mode
- """
+ position_cmd = []
+ for idx, name in enumerate(self.arm._joint_names):
+ pos = self.joint_position[name]
+ cmd = delta_cmd[idx]
+ position_cmd.append(pos + cmd)
+ transformed_delta_vel = self._transform_panda_list_to_dict(position_cmd[0:7])
- # msg = JointTrajectory()
- # msg.header = Header()
- # msg.joint_names = copy.deepcopy(self.arm._joint_names)
- # msg.points = [JointTrajectoryPoint()]
- # msg.points[0].positions = position_cmd
- # duration = []
- # for i in range(len(msg.joint_names)):
- # TODO check if the index is in bounds
- # !!! Be careful with panda_state index here
- # pos = self.panda_state[i]
- # cmd = position_cmd[i]
- # max_vel = self.panda_joint_vel_limits[i]
- # temp_duration = max(abs(cmd - pos) / max_vel, self.min_traj_duration)
- # duration.append(temp_duration)
-
- # msg.points[0].time_from_start = rospy.Duration.from_sec(max(duration))
- # print(msg)
-
- # self.arm_cmd_pub.publish(msg)
-
- transformed_j_pos = self._transform_panda_list_to_dict(position_cmd[0:7])
- self.arm.set_joint_positions(transformed_j_pos)
+ self.arm.set_joint_velocities(transformed_delta_vel)
+ #self.arm.move_to_joint_positions(transformed_delta_pos, use_moveit=False)
+
rospy.sleep(self.control_period)
- return position_cmd
+
+ def publish_env_arm_delta_vel_cmd(self, delta_cmd): # Velocity controller in difference mode.
+ velocity_cmd = []
+ for idx, name in enumerate(self.arm._joint_names):
+ vel = self.joint_velocity[name]
+ cmd = delta_cmd[idx]
+ vel_cmd_i = vel + cmd
+ vel_cmd_sign = np.sign(vel_cmd_i)
+ if abs(vel_cmd_i) >= self.velocity_limits[idx]:
+ vel_cmd_i = vel_cmd_sign * self.velocity_limits[idx]
+ velocity_cmd.append(vel_cmd_i)
+
+
+ transformed_delta_vel = self._transform_panda_list_to_dict(velocity_cmd[0:7])
+ self.arm.set_joint_velocities(transformed_delta_vel)
+
+ #rospy.sleep(self.control_period)
+
+ def publish_vel_cmd_linear_to_zero(self): # NOT USED: To solve the issue for robot velocity message suddenly stopping in real robot
+ delta_cmd = 0.002
+ velocity_cmd = []
+ while True:
+
+ for name in self.arm._joint_names:
+ vel = self.joint_velocity[name]
+ if vel > 0.003:
+ vel_cmd_i = 0
+ elif vel < -0.003:
+ vel_cmd_i = 0
+ else:
+ vel_cmd_i = 0
+ velocity_cmd.append(vel_cmd_i)
+
+
+
+ transformed_delta_vel = self._transform_panda_list_to_dict(velocity_cmd[0:7])
+ self.arm.set_joint_velocities(transformed_delta_vel)
+
+ vel_list = self._get_velocity_list()
+ if all(v<=0.003 for v in vel_list):
+ rospy.sleep(self.control_period)
+ Break
+ rospy.sleep(self.control_period)
+
+
def _transform_panda_list_to_dict(self, panda_list):
transformed_dict = {}
for idx, value in enumerate(panda_list):
@@ -326,7 +402,14 @@ def _get_joint_position_dict_from_rs_dict(self, rs_dict):
d['panda_joint7'] = rs_dict['joint7_position']
return d
-
+
+ def _get_velocity_list(self):
+ vels=[]
+ for name in self.arm._joint_names:
+ vel = self.joint_velocity[name]
+ vels.append(vel)
+ return vels
+
def _on_link1_collision(self, data):
if data.states == []:
pass
@@ -380,4 +463,4 @@ def _on_rightfinger_collision(self, data):
pass
else:
self.collision_sensors['panda_rightfinger'] = True
-
\ No newline at end of file
+
diff --git a/simulation_objects/scripts/objects_controller.py b/simulation_objects/scripts/objects_controller.py
index e98a60e..76e1578 100755
--- a/simulation_objects/scripts/objects_controller.py
+++ b/simulation_objects/scripts/objects_controller.py
@@ -243,11 +243,83 @@ def get_fixed_position_a_b(self, x_a, y_a, z_a, x_b, y_b, z_b, hold_a, hold_b):
return x_function, y_function, z_function
+ def get_wait_and_3D_spline(self, x_min, x_max, y_min, y_max, z_min, z_max, hold_a, n_points = 10, n_sampling_points = 4000):
+
+
+ """Generate samples of the cartesian coordinates of a 3d spline that do not cross a vertical
+ cylinder of radius r_min centered in 0,0.
+
+ Args:
+ x_min (float): min x coordinate of random points used to interpolate spline (m).
+ x_max (float): max x coordinate of random points used to interpolate spline (m).
+ y_min (float): min y coordinate of random points used to interpolate spline (m).
+ y_max (float): max y coordinate of random points used to interpolate spline (m).
+ z_min (float): min z coordinate of random points used to interpolate spline (m).
+ z_max (float): max z coordinate of random points used to interpolate spline (m).
+ n_points (int): number of random points used to interpolate the 3d spline.
+ n_sampling_points (int): number of the samples to take over the whole length of the spline.
+
+ Returns:
+ np.array: Samples of the x coordinate of the function over time.
+ np.array: Samples of the y coordinate of the function over time.
+ np.array: Samples of the z coordinate of the function over time.
+
+ """
+
+ r_min_cylinder = 0.2
+ r_min_sphere_base = 0.35
+
+ # Convert number of points to int
+ n_points = int(n_points)
+ # Convert number of sampling points to int
+ # By increasing the number of sampling points the speed of the object decreases
+ n_sampling_points = int(n_sampling_points)
+ # Create array with time samples over 1 full function period
+
+ search = True
+ while search:
+ x = np.random.uniform(x_min,x_max,n_points)
+ y = np.random.uniform(y_min,y_max,n_points)
+ z = np.random.uniform(z_min,z_max,n_points)
+
+ # set first point oustide of square of size 0.5m centered in 0,0
+ #x[0] = random.choice([np.random.uniform(-1.0,-0.5),np.random.uniform(0.5,1.0)])
+ #y[0] = random.choice([np.random.uniform(-1.0,-0.5),np.random.uniform(0.5,1.0)])
+
+ # set last point equal to first to have a closed trajectory
+ x[n_points-1] = x[0]
+ y[n_points-1] = y[0]
+ z[n_points-1] = z[0]
+
+ x_a_function = np.full(int(self.update_rate * hold_a), x[0])
+ y_a_function = np.full(int(self.update_rate * hold_a), y[0])
+ z_a_function = np.full(int(self.update_rate * hold_a), z[0])
+
+
+ smoothness = 0
+ tck, u = interpolate.splprep([x,y,z], s=smoothness)
+ u_fine = np.linspace(0,1,n_sampling_points)
+ x_function, y_function, z_function = interpolate.splev(u_fine, tck)
+
+ x_function = np.concatenate((x_a_function, x_function))
+ y_function = np.concatenate((y_a_function, y_function))
+ z_function = np.concatenate((z_a_function, z_function))
+
+
+ search = False
+ for i in range(len(x_function)):
+ if (x_function[i]**2+y_function[i]**2)**(1/2) <= r_min_cylinder or \
+ (x_function[i]**2+y_function[i]**2+z_function[i]**2)**(1/2) <= r_min_sphere_base :
+ search = True
+
+
+ return x_function, y_function, z_function
+
def get_interpolated_a_b_c(self, x_a, y_a, z_a, x_b, y_b, z_b, x_c, y_c, z_c, hold_a, hold_b, hold_c, n_sampling_points_ab, n_sampling_points_bc):
"""Generate trajectory for object:
- a for hold_a time
- move from a to b
- - b for hold_c time
+ - b for hold_b time
- move from b to c
- c for hold_c time
@@ -277,7 +349,7 @@ def get_interpolated_a_b_c(self, x_a, y_a, z_a, x_b, y_b, z_b, x_c, y_c, z_c, ho
z_a_function = np.full(int(self.update_rate * hold_a), z_a)
tck_ab, _ = interpolate.splprep([[x_a,x_b],[y_a,y_b],[z_a,z_b]], s=0, k=1)
- u_fine_ab = np.linspace(0, 1, n_sampling_points_ab)
+ u_fine_ab = np.linspace(0, 1, int(n_sampling_points_ab))
x_ab_function, y_ab_function, z_ab_function = interpolate.splev(u_fine_ab, tck_ab)
x_b_function = np.full(int(self.update_rate * hold_b), x_b)
@@ -285,7 +357,7 @@ def get_interpolated_a_b_c(self, x_a, y_a, z_a, x_b, y_b, z_b, x_c, y_c, z_c, ho
z_b_function = np.full(int(self.update_rate * hold_b), z_b)
tck_bc, _ = interpolate.splprep([[x_b,x_c],[y_b,y_c],[z_b,z_c]], s=0, k=1)
- u_fine_bc = np.linspace(0, 1, n_sampling_points_bc)
+ u_fine_bc = np.linspace(0, 1, int(n_sampling_points_bc))
x_bc_function, y_bc_function, z_bc_function = interpolate.splev(u_fine_bc, tck_bc)
x_c_function = np.full(int(self.update_rate * hold_c), x_c)
@@ -298,6 +370,32 @@ def get_interpolated_a_b_c(self, x_a, y_a, z_a, x_b, y_b, z_b, x_c, y_c, z_c, ho
return x_function, y_function, z_function
+ def get_interpolated_a_b_a(self, x_a, y_a, z_a, x_b, y_b, z_b, hold_a, hold_b, n_sampling_points):
+
+ # Added by FARHANG for defining target path in reach tasks
+ x_a_function = np.full(int(self.update_rate * hold_a), x_a)
+ y_a_function = np.full(int(self.update_rate * hold_a), y_a)
+ z_a_function = np.full(int(self.update_rate * hold_a), z_a)
+
+ tck_ab, _ = interpolate.splprep([[x_a,x_b],[y_a,y_b],[z_a,z_b]], s=0, k=1)
+ u_fine_ab = np.linspace(0, 1, int(n_sampling_points))
+ x_ab_function, y_ab_function, z_ab_function = interpolate.splev(u_fine_ab, tck_ab)
+
+ x_b_function = np.full(int(self.update_rate * hold_b), x_b)
+ y_b_function = np.full(int(self.update_rate * hold_b), y_b)
+ z_b_function = np.full(int(self.update_rate * hold_b), z_b)
+
+ tck_ba, _ = interpolate.splprep([[x_b,x_a],[y_b,y_a],[z_b,z_a]], s=0, k=1)
+ u_fine_ba = np.linspace(0, 1, int(n_sampling_points))
+ x_ba_function, y_ba_function, z_ba_function = interpolate.splev(u_fine_ba, tck_ba)
+
+
+ x_function = np.concatenate((x_a_function, x_ab_function, x_b_function, x_ba_function, x_a_function))
+ y_function = np.concatenate((y_a_function, y_ab_function, y_b_function, y_ba_function, y_a_function))
+ z_function = np.concatenate((z_a_function, z_ab_function, z_b_function, z_ba_function, z_a_function))
+
+ return x_function, y_function, z_function
+
def objects_initialization(self):
self.n_objects = int(rospy.get_param("n_objects", 1))
# Initialization of ModelState() messages
@@ -374,6 +472,17 @@ def objects_state_update_loop(self):
n_points = rospy.get_param("object_" + repr(i) + "_n_points")
n_sampling_points = rospy.get_param("object_" + repr(i) +"_n_sampling_points")
x_trajectory, y_trajectory, z_trajectory = self.get_3d_spline_ur5_workspace(x_min, x_max, y_min, y_max, z_min, z_max, n_points, n_sampling_points)
+ elif function == "wait_and_3D_spline":
+ x_min = rospy.get_param("object_" + repr(i) + "_x_min")
+ x_max = rospy.get_param("object_" + repr(i) + "_x_max")
+ y_min = rospy.get_param("object_" + repr(i) + "_y_min")
+ y_max = rospy.get_param("object_" + repr(i) + "_y_max")
+ z_min = rospy.get_param("object_" + repr(i) + "_z_min")
+ z_max = rospy.get_param("object_" + repr(i) + "_z_max")
+ hold_a = rospy.get_param("object_" + repr(i) + "_hold_a")
+ n_points = rospy.get_param("object_" + repr(i) + "_n_points")
+ n_sampling_points = rospy.get_param("object_" + repr(i) +"_n_sampling_points")
+ x_trajectory, y_trajectory, z_trajectory = self.get_wait_and_3D_spline(x_min, x_max, y_min, y_max, z_min, z_max, hold_a, n_points, n_sampling_points)
elif function == "fixed_trajectory":
trajectory_id = rospy.get_param("object_" + repr(i) + "_trajectory_id")
x_trajectory, y_trajectory, z_trajectory = self.get_fixed_trajectory(trajectory_id)
@@ -403,6 +512,19 @@ def objects_state_update_loop(self):
n_sampling_points_ab = rospy.get_param("object_" + repr(i) + "_n_sampling_points_ab")
n_sampling_points_bc = rospy.get_param("object_" + repr(i) + "_n_sampling_points_bc")
x_trajectory, y_trajectory, z_trajectory = self.get_interpolated_a_b_c(x_a, y_a, z_a, x_b, y_b, z_b, x_c, y_c, z_c, hold_a, hold_b, hold_c, n_sampling_points_ab, n_sampling_points_bc)
+ elif function == "interpolated_aba": # Added by FARHANG for defining target path in reach tasks
+ x_a = rospy.get_param("object_" + repr(i) + "_x_a")
+ y_a = rospy.get_param("object_" + repr(i) + "_y_a")
+ z_a = rospy.get_param("object_" + repr(i) + "_z_a")
+ x_b = rospy.get_param("object_" + repr(i) + "_x_b")
+ y_b = rospy.get_param("object_" + repr(i) + "_y_b")
+ z_b = rospy.get_param("object_" + repr(i) + "_z_b")
+ hold_a = rospy.get_param("object_" + repr(i) + "_hold_a")
+ hold_b = rospy.get_param("object_" + repr(i) + "_hold_b")
+ n_sampling_points = rospy.get_param("object_" + repr(i) + "_n_sampling_points")
+
+ x_trajectory, y_trajectory, z_trajectory = self.get_interpolated_a_b_a(x_a, y_a, z_a, x_b, y_b, z_b, hold_a, hold_b, n_sampling_points)
+
else:
rospy.logerr('Object trajectory function "' +function+ '" not recognized')
objects_trajectories.append([x_trajectory, y_trajectory, z_trajectory])
diff --git a/simulation_objects/worlds/empty.world b/simulation_objects/worlds/empty.world
index 172d41c..05e23f6 100644
--- a/simulation_objects/worlds/empty.world
+++ b/simulation_objects/worlds/empty.world
@@ -14,5 +14,8 @@
1
1000
+
+ 0.0 0.0 0.0
+
diff --git a/simulation_objects/worlds/tabletop_sphere50_no_collision.world b/simulation_objects/worlds/tabletop_sphere50_no_collision.world
index 1b57970..d54a405 100644
--- a/simulation_objects/worlds/tabletop_sphere50_no_collision.world
+++ b/simulation_objects/worlds/tabletop_sphere50_no_collision.world
@@ -24,5 +24,8 @@
1
1000
+
+ 0.0 0.0 0.0
+