diff --git a/panda_gazebo/launch/put_robot_in_world.launch b/panda_gazebo/launch/put_robot_in_world.launch
index 02c4a7b9..377cf6a2 100644
--- a/panda_gazebo/launch/put_robot_in_world.launch
+++ b/panda_gazebo/launch/put_robot_in_world.launch
@@ -6,7 +6,7 @@
-
+
diff --git a/panda_gazebo/src/panda_gazebo/common/helpers.py b/panda_gazebo/src/panda_gazebo/common/helpers.py
index f76a2c92..90f431ce 100644
--- a/panda_gazebo/src/panda_gazebo/common/helpers.py
+++ b/panda_gazebo/src/panda_gazebo/common/helpers.py
@@ -8,13 +8,13 @@
import numpy as np
import rospy
from actionlib_msgs.msg import GoalStatusArray
+from controller_manager_msgs.srv import ListControllersResponse
+from geometry_msgs.msg import Quaternion
+from moveit_msgs.msg import MoveItErrorCodes
from numpy import linalg, nan
from rospy.exceptions import ROSException
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectoryPoint
-from controller_manager_msgs.srv import ListControllersResponse
-from moveit_msgs.msg import MoveItErrorCodes
-from geometry_msgs.msg import Quaternion
from panda_gazebo.msg import FollowJointTrajectoryGoal
@@ -237,7 +237,7 @@ def translate_moveit_error_code(moveit_error_code):
if attr[0] != "_" and all(map(str.isupper, attr.replace("_", "")))
}
return (
- error_dict[moveit_error_code.val].lower().capitalize().replace("_", " ") + "."
+ error_dict[moveit_error_code.val].lower().capitalize().replace("_", " ")
if error_dict[moveit_error_code.val] != "SUCCESSFUL"
else ""
)
diff --git a/panda_gazebo/src/panda_gazebo/core/control_server.py b/panda_gazebo/src/panda_gazebo/core/control_server.py
index bb159445..39093733 100644
--- a/panda_gazebo/src/panda_gazebo/core/control_server.py
+++ b/panda_gazebo/src/panda_gazebo/core/control_server.py
@@ -1155,6 +1155,11 @@ def _joint_states_cb(self, data):
def _set_joint_commands_cb(self, set_joint_commands_req):
"""Request arm and hand joint command control.
+ .. note::
+ The gripper `max_effort` is determined based on the `grasping` field if it's
+ set to `0`. Specifically, if `grasping` is `True`, `max_effort` is set to
+ 10N. Otherwise, it remains at `0`.
+
Args:
set_joint_commands_req (:obj:`panda_gazebo.srv.SetJointPositionsRequest`):
Service request message specifying the joint position/effort commands
@@ -1444,6 +1449,11 @@ def _arm_set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901
def _set_gripper_width_cb(self, set_gripper_width_req):
"""Request gripper width.
+ .. note::
+ The gripper `max_effort` is determined based on the `grasping` field if it's
+ set to `0`. Specifically, if `grasping` is `True`, `max_effort` is set to
+ 10N. Otherwise, it remains at `0`.
+
Args:
set_gripper_width_req (:obj:`panda_gazebo.srv.SetGripperWidth`):
Service request message specifying the gripper width for the robot hand.
@@ -1462,15 +1472,13 @@ def _set_gripper_width_cb(self, set_gripper_width_req):
)
# Create gripper command action message.
- # NOTE: The max_effort has to be 0 for the gripper to move (see #33).
+ # NOTE: The max_effort has to be 0 for the gripper to be able to move (see #33).
req = GripperCommandGoal()
req.command.position = gripper_width
req.command.max_effort = (
- (
- set_gripper_width_req.max_effort
- if set_gripper_width_req.max_effort != 0.0
- else GRASP_FORCE
- )
+ set_gripper_width_req.max_effort
+ if set_gripper_width_req.max_effort != 0.0
+ else GRASP_FORCE
if set_gripper_width_req.grasping
else 0.0
)
diff --git a/panda_gazebo/src/panda_gazebo/core/moveit_server.py b/panda_gazebo/src/panda_gazebo/core/moveit_server.py
index 6de058d5..67fb229a 100644
--- a/panda_gazebo/src/panda_gazebo/core/moveit_server.py
+++ b/panda_gazebo/src/panda_gazebo/core/moveit_server.py
@@ -47,13 +47,13 @@
get_duplicate_list,
get_unique_list,
joint_state_dict_2_joint_state_msg,
+ load_panda_joint_limits,
lower_first_char,
normalize_quaternion,
+ normalize_vector,
quaternion_norm,
ros_exit_gracefully,
translate_moveit_error_code,
- normalize_vector,
- load_panda_joint_limits,
)
from panda_gazebo.exceptions import InputMessageInvalidError, JointLimitsInvalidError
from panda_gazebo.srv import (
@@ -120,7 +120,7 @@ def __init__( # noqa: C901
Args:
arm_move_group (str, optional): The name of the move group you want to use
for controlling the Panda arm. Defaults to ``panda_arm``.
- arm_ee_link (str, optional): The end effector you want moveit to use when
+ arm_ee_link (str, optional): The end effector you want MoveIt to use when
controlling the Panda arm. Defaults to ``panda_link8``.
hand_move_group (str, optional): The name of the move group you want to use
for controlling the Panda hand. Defaults to ``panda_hand``.
@@ -472,8 +472,11 @@ def _execute(self, control_group="both", wait=True): # noqa: C901
wait (boolean, optional): Whether to wait on the control to be executed.
Returns:
- list: List specifying whether the arm and/or hand execution was successful.
+ (tuple): tuple containing:
+
+ - list: List specifying whether the arm and/or hand execution was successful.
If ``control_group == "both"`` then ``["arm_success", "hand_success"]``.
+ - str: The error message, or an empty string if successful.
"""
control_group = control_group.lower()
if control_group not in ["arm", "hand", "both"]:
@@ -484,7 +487,7 @@ def _execute(self, control_group="both", wait=True): # noqa: C901
return [False]
# Plan and execute trajectory.
- retval, groups = [], []
+ retval, groups, err_msg = [], [], None
if control_group in ["arm", "both"]:
groups.append(self.move_group_arm)
if control_group in ["hand", "both"]:
@@ -501,14 +504,15 @@ def _execute(self, control_group="both", wait=True): # noqa: C901
if wait:
group.stop()
else:
+ err_msg = translate_moveit_error_code(error_code)
rospy.logwarn(
f"No plan found for the current {group.get_name()} setpoints "
- f"since '{translate_moveit_error_code(error_code)}'."
+ f"since '{err_msg}'."
)
group_retval = False
retval.append(group_retval)
- return retval
+ return retval, err_msg
def _create_positions_moveit_setpoints( # noqa: C901
self, set_joint_positions_req, control_group
@@ -1043,7 +1047,7 @@ def _arm_get_ee_pose_joint_config(self, get_ee_pose_joint_configuration):
# Make sure quaternion is normalized.
if quaternion_norm(get_ee_pose_joint_configuration.pose.orientation) != 1.0:
rospy.logwarn(
- "The quaternion in the set ee pose was normalized since moveit expects "
+ "The quaternion in the set ee pose was normalized since MoveIt expects "
"normalized quaternions."
)
get_ee_pose_joint_configuration.pose.orientation = normalize_quaternion(
@@ -1101,7 +1105,7 @@ def _arm_set_ee_pose_callback(self, set_ee_pose_req):
# Make sure quaternion is normalized.
if quaternion_norm(set_ee_pose_req.pose.orientation) != 1.0:
rospy.logwarn(
- "The quaternion in the set ee pose was normalized since moveit expects "
+ "The quaternion in the set ee pose was normalized since MoveIt expects "
"normalized quaternions."
)
set_ee_pose_req.pose.orientation = normalize_quaternion(
@@ -1116,13 +1120,16 @@ def _arm_set_ee_pose_callback(self, set_ee_pose_req):
# Send trajectory message and return response.
try:
self.move_group_arm.set_pose_target(self.ee_pose_target)
- retval = self._execute(control_group="arm")
+ retval, err_msg = self._execute(control_group="arm")
self.move_group_arm.clear_pose_targets()
# Check if setpoint execution was successful.
if not all(retval):
resp.success = False
- resp.message = "Ee pose could not be set"
+ resp.message = (
+ "Ee pose could not be set since MoveIt "
+ f"{lower_first_char(err_msg)}."
+ )
else:
resp.success = True
resp.message = "Everything went OK"
@@ -1192,10 +1199,12 @@ def _set_joint_positions_callback(self, set_joint_positions_req): # noqa: C901
# Execute setpoints.
rospy.logdebug("Executing joint positions setpoint.")
try:
- retval = self._execute()
+ retval, err_msg = self._execute()
if not all(retval):
resp.success = False
- resp.message = "Joint position setpoint could not executed"
+ resp.message = (
+ f"Joint position setpoint could not executed since {err_msg}."
+ )
else:
resp.success = True
resp.message = "Everything went OK"
@@ -1254,10 +1263,12 @@ def _arm_set_joint_positions_callback(self, set_joint_positions_req):
# Execute setpoint.
rospy.logdebug("Executing joint positions setpoint.")
try:
- retval = self._execute(control_group="arm")
+ retval, err_msg = self._execute(control_group="arm")
if not all(retval):
resp.success = False
- resp.message = "Arm joint position setpoint could not executed"
+ resp.message = (
+ f"Arm joint position setpoint could not executed since {err_msg}."
+ )
else:
resp.success = True
resp.message = "Everything went OK"
@@ -1314,10 +1325,13 @@ def _hand_set_joint_positions_callback(self, set_joint_positions_req):
# Execute setpoints.
rospy.logdebug("Executing joint positions setpoint.")
try:
- retval = self._execute(control_group="hand")
+ retval, err_msg = self._execute(control_group="hand")
if not all(retval):
resp.success = False
- resp.message = "Hand joint position setpoint could not be executed"
+ resp.message = (
+ "Hand joint position setpoint could not be executed since "
+ f"{err_msg}."
+ )
else:
resp.success = True
resp.message = "Everything went OK"
@@ -1660,7 +1674,7 @@ def _get_random_ee_pose_callback(self, get_random_ee_pose_req):
return resp
def _get_controlled_joints_cb(self, _):
- """Returns the joints that are currently being controlled by moveit.
+ """Returns the joints that are currently being controlled by MoveIt.
Args:
get_controlled_joints_req (:obj:`panda_gazebo.srv.GetControlledJointsRequest`):
diff --git a/panda_gazebo/srv/AddBox.srv b/panda_gazebo/srv/AddBox.srv
index 7a48a39c..14497ed8 100755
--- a/panda_gazebo/srv/AddBox.srv
+++ b/panda_gazebo/srv/AddBox.srv
@@ -5,4 +5,4 @@ geometry_msgs/Pose pose
float64[3] size
---
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/AddPlane.srv b/panda_gazebo/srv/AddPlane.srv
index ea75704f..db8bb8d8 100755
--- a/panda_gazebo/srv/AddPlane.srv
+++ b/panda_gazebo/srv/AddPlane.srv
@@ -6,4 +6,4 @@ float64[3] normal
float64 offset # In the direction of the normal
---
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/GetControlledJoints.srv b/panda_gazebo/srv/GetControlledJoints.srv
index b9db4715..330bfd76 100644
--- a/panda_gazebo/srv/GetControlledJoints.srv
+++ b/panda_gazebo/srv/GetControlledJoints.srv
@@ -6,4 +6,4 @@ string[] controlled_joints
string[] controlled_joints_arm
string[] controlled_joints_hand
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/GetEe.srv b/panda_gazebo/srv/GetEe.srv
index 6dc91845..861347a6 100755
--- a/panda_gazebo/srv/GetEe.srv
+++ b/panda_gazebo/srv/GetEe.srv
@@ -1,6 +1,6 @@
-# Service that can be used to get the current EE used in moveit.
+# Service that can be used to get the current EE used in MoveIt.
---
string ee_name
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/GetEePose.srv b/panda_gazebo/srv/GetEePose.srv
index f5074174..c2a07c28 100755
--- a/panda_gazebo/srv/GetEePose.srv
+++ b/panda_gazebo/srv/GetEePose.srv
@@ -1,6 +1,6 @@
-# Service that can be used to get pose information about the EE using moveit.
+# Service that can be used to get pose information about the EE using MoveIt.
---
geometry_msgs/Pose pose
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/GetEePoseJointConfig.srv b/panda_gazebo/srv/GetEePoseJointConfig.srv
index fe93c989..346a84a0 100755
--- a/panda_gazebo/srv/GetEePoseJointConfig.srv
+++ b/panda_gazebo/srv/GetEePoseJointConfig.srv
@@ -1,9 +1,9 @@
# Service that can be used to get a set of posible joint configuration for a given EE
-# pose using moveit.
+# pose using MoveIt.
geometry_msgs/Pose pose
int32 attempts
---
string[] joint_names
float64[] joint_positions
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/GetEeRpy.srv b/panda_gazebo/srv/GetEeRpy.srv
index 3a6a8de9..91e1ca83 100755
--- a/panda_gazebo/srv/GetEeRpy.srv
+++ b/panda_gazebo/srv/GetEeRpy.srv
@@ -1,8 +1,8 @@
-# Service that can be used to get orientation information about the EE using moveit.
+# Service that can be used to get orientation information about the EE using MoveIt.
---
float32 r
float32 p
float32 y
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/GetMoveItControlledJoints.srv b/panda_gazebo/srv/GetMoveItControlledJoints.srv
index a5c65a0f..e3fd4ae2 100644
--- a/panda_gazebo/srv/GetMoveItControlledJoints.srv
+++ b/panda_gazebo/srv/GetMoveItControlledJoints.srv
@@ -4,4 +4,4 @@ string[] controlled_joints
string[] controlled_joints_arm
string[] controlled_joints_hand
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/GetRandomEePose.srv b/panda_gazebo/srv/GetRandomEePose.srv
index a8fa9c2f..7d37534d 100644
--- a/panda_gazebo/srv/GetRandomEePose.srv
+++ b/panda_gazebo/srv/GetRandomEePose.srv
@@ -6,4 +6,4 @@ geometry_msgs/Pose ee_pose
string[] joint_names
float64[] joint_positions
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/GetRandomJointPositions.srv b/panda_gazebo/srv/GetRandomJointPositions.srv
index cfbf9b4b..945ce20a 100644
--- a/panda_gazebo/srv/GetRandomJointPositions.srv
+++ b/panda_gazebo/srv/GetRandomJointPositions.srv
@@ -6,4 +6,4 @@ int32 attempts
string[] joint_names
float64[] joint_positions
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/SetEe.srv b/panda_gazebo/srv/SetEe.srv
index f7125912..f6e9ccb3 100755
--- a/panda_gazebo/srv/SetEe.srv
+++ b/panda_gazebo/srv/SetEe.srv
@@ -1,5 +1,5 @@
-# Service that can be used to set the current EE used in moveit.
+# Service that can be used to set the current EE used in MoveIt.
string ee_name
---
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/SetEePose.srv b/panda_gazebo/srv/SetEePose.srv
index 33668d6b..f70ca26d 100755
--- a/panda_gazebo/srv/SetEePose.srv
+++ b/panda_gazebo/srv/SetEePose.srv
@@ -1,6 +1,6 @@
# Service that can be used to set the current EE pose and control the robot using
-# moveit.
+# MoveIt.
geometry_msgs/Pose pose
---
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/SetGripperWidth.srv b/panda_gazebo/srv/SetGripperWidth.srv
index fed3b7e0..26b72974 100755
--- a/panda_gazebo/srv/SetGripperWidth.srv
+++ b/panda_gazebo/srv/SetGripperWidth.srv
@@ -1,5 +1,5 @@
# Service that can be used to control the robot hand gripper width using the panda_control_server.
-# NOTE: It serves as a small wrapper around the 'franka_gripper/move` action but automatically
+# NOTE: It serves as a small wrapper around the 'franka_gripper/move' action but automatically
# sets the speed to the maximum speed. It further clips gripper width such that it is within
# the set max/min boundaries.
float64 width # Gripper width - ignored when the gripper is grasping.
diff --git a/panda_gazebo/srv/SetJointCommands.srv b/panda_gazebo/srv/SetJointCommands.srv
index 834be457..f5dfd240 100755
--- a/panda_gazebo/srv/SetJointCommands.srv
+++ b/panda_gazebo/srv/SetJointCommands.srv
@@ -4,10 +4,10 @@
# the gripper width to the 'franka_gripper/move' action service.
string[] joint_names
float64[] joint_commands
-string control_type # The control type you want to use for the robot arm. Options are "effort" and "position".
-bool grasping # The gripper simply moves if this is `false`.
+string control_type # The control type you want to use for the robot arm. Options are `effort` and `position`.
+bool grasping # If set to true, the gripper 'max_effort' will be set to 10N when not supplied. This will simplify grasping.
bool arm_wait # Wait till the arm control has completed
bool hand_wait # Wait till the hand control has completed
---
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/SetJointEfforts.srv b/panda_gazebo/srv/SetJointEfforts.srv
index 3821f548..553f7b25 100755
--- a/panda_gazebo/srv/SetJointEfforts.srv
+++ b/panda_gazebo/srv/SetJointEfforts.srv
@@ -5,4 +5,4 @@ float64[] joint_efforts
bool wait
---
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/srv/SetJointPositions.srv b/panda_gazebo/srv/SetJointPositions.srv
index 82eaa045..ae86c083 100755
--- a/panda_gazebo/srv/SetJointPositions.srv
+++ b/panda_gazebo/srv/SetJointPositions.srv
@@ -6,4 +6,4 @@ float64[] joint_positions
bool wait
---
bool success
-string message
\ No newline at end of file
+string message
diff --git a/panda_gazebo/tests/manual/test_effort_joint_trajectory_controller.py b/panda_gazebo/tests/manual/test_effort_joint_trajectory_controller.py
index 58a475ad..8491766a 100644
--- a/panda_gazebo/tests/manual/test_effort_joint_trajectory_controller.py
+++ b/panda_gazebo/tests/manual/test_effort_joint_trajectory_controller.py
@@ -7,7 +7,6 @@
from std_msgs.msg import Header
from trajectory_msgs.msg import JointTrajectoryPoint
-
if __name__ == "__main__":
rospy.init_node("position_joint_trajectory_controller_test")
diff --git a/panda_gazebo/tests/manual/test_joint_position_group_controller.py b/panda_gazebo/tests/manual/test_joint_position_group_controller.py
new file mode 100644
index 00000000..55470360
--- /dev/null
+++ b/panda_gazebo/tests/manual/test_joint_position_group_controller.py
@@ -0,0 +1,35 @@
+"""Small script that can be used to checkout how the robot moves when sending
+a sinusoid command to one of the panda joints using the position joint group controller.
+"""
+import time
+
+import numpy as np
+import rospy
+from std_msgs.msg import Float64MultiArray
+
+if __name__ == "__main__":
+ rospy.init_node("position_group_joint_controller_test")
+
+ # Create joint controller publisher.
+ arm_joint_group_controller_pub = rospy.Publisher(
+ "/panda_arm_joint_position_controller/command",
+ Float64MultiArray,
+ queue_size=10,
+ )
+
+ # Send sinonoidal command to two joints.
+ t_start = time.time()
+ while not rospy.is_shutdown():
+ msg = Float64MultiArray()
+ msg.data = [
+ 0,
+ 0,
+ 2.9671 * np.sin(time.time() - t_start),
+ 2.9671 * np.sin(time.time() - t_start),
+ 0,
+ 0,
+ 0,
+ ]
+ arm_joint_group_controller_pub.publish(msg)
+ time.sleep(0.01)
+ print("Done!")
diff --git a/panda_gazebo/tests/manual/test_joint_position_trajectory_controller.py b/panda_gazebo/tests/manual/test_joint_position_trajectory_controller.py
index 7884ceaa..8ae00d26 100644
--- a/panda_gazebo/tests/manual/test_joint_position_trajectory_controller.py
+++ b/panda_gazebo/tests/manual/test_joint_position_trajectory_controller.py
@@ -9,6 +9,7 @@
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from trajectory_msgs.msg import JointTrajectoryPoint
+
from panda_gazebo.common.helpers import ros_exit_gracefully
TIMESTEP = 0.01
@@ -71,7 +72,6 @@ def periodic_test(trajectory_client):
def position_test(client):
"""Send a position command to the robot using the trajectory controller."""
-
# Create action client goal.
header = Header()
# header.stamp = rospy.get_rostime()
diff --git a/panda_gazebo/tests/manual/moveit_commander_test.py b/panda_gazebo/tests/manual/test_moveit_commander.py
similarity index 52%
rename from panda_gazebo/tests/manual/moveit_commander_test.py
rename to panda_gazebo/tests/manual/test_moveit_commander.py
index 0f958834..5c6905b8 100755
--- a/panda_gazebo/tests/manual/moveit_commander_test.py
+++ b/panda_gazebo/tests/manual/test_moveit_commander.py
@@ -1,13 +1,13 @@
#!/usr/bin/env python3
"""Small script for testing capabilities of the MoveIt commander."""
-import sys
import math
+import sys
-from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Point
import moveit_commander
import moveit_msgs.msg
-from std_msgs.msg import Header
import rospy
+from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion
+from std_msgs.msg import Header
if __name__ == "__main__":
# Initialise MoveIt commander and node.
@@ -22,6 +22,7 @@
# Load arm and hand groups.
move_group = moveit_commander.MoveGroupCommander("panda_arm")
hand_move_group = moveit_commander.MoveGroupCommander("panda_hand")
+ full_move_group = moveit_commander.MoveGroupCommander("panda_manipulator")
display_trajectory_publisher = rospy.Publisher(
"/move_group/display_planned_path",
@@ -47,56 +48,56 @@
print(robot.get_current_state())
print("")
- # # -- Plan arm joint goal --
-
- # # We get the joint values from the group and change some of the values:
- # joint_goal = move_group.get_current_joint_values()
- # joint_goal[0] = 0
- # joint_goal[1] = -math.tau / 8
- # joint_goal[2] = 0
- # joint_goal[3] = -math.tau / 4
- # joint_goal[4] = 0
- # joint_goal[5] = math.tau / 6 # 1/6 of a turn
- # joint_goal[6] = 0
-
- # # METHOD 1
- # # -- UNCOMMENT BELOW TO SEE THE SCALING FACTOR IN ACTION --
- # # max_vel_scale_factor = 0.2
- # # max_vel_scale_factor = 1.0
- # # max_acc_scale_factor = 0.2
- # # max_acc_scale_factor = 1.0
- # # move_group.set_max_velocity_scaling_factor(max_vel_scale_factor)
- # # move_group.set_max_acceleration_scaling_factor(max_acc_scale_factor)
- # # -- UNCOMMENT ABOVE TO SEE THE SCALING FACTOR IN ACTION --
- # move_group.set_joint_value_target(joint_goal)
- # move_group.plan()
- # move_group.go(wait=True)
-
- # # METHOD 2
- # # The go command can be called with joint values, poses, or without any
- # # parameters if you have already set the pose or joint target for the group.
- # move_group.go(joint_goal, wait=True)
-
- # # Calling ``stop()`` ensures that there is no residual movement
- # move_group.stop()
-
- # # # -- Plan pose goal --
- # pose_goal = Pose()
- # pose_goal.orientation.w = 1.0
- # pose_goal.position.x = 400
- # pose_goal.position.y = 100
- # pose_goal.position.z = 0.4
-
- # # move_group.set_pose_target(pose_goal)
- # retval, plan, _, error_code = move_group.plan(pose_goal)
- # plan = move_group.go(wait=True)
-
- # # Calling `stop()` ensures that there is no residual movement
- # move_group.stop()
-
- # # It is always good to clear your targets after planning with poses.
- # # Note: there is no equivalent function for clear_joint_value_targets()
- # move_group.clear_pose_targets()
+ # -- Plan arm joint goal --
+
+ # We get the joint values from the group and change some of the values:
+ joint_goal = move_group.get_current_joint_values()
+ joint_goal[0] = 0
+ joint_goal[1] = -math.tau / 8
+ joint_goal[2] = 0
+ joint_goal[3] = -math.tau / 4
+ joint_goal[4] = 0
+ joint_goal[5] = math.tau / 6 # 1/6 of a turn
+ joint_goal[6] = 0
+
+ # METHOD 1
+ # -- UNCOMMENT BELOW TO SEE THE SCALING FACTOR IN ACTION --
+ # max_vel_scale_factor = 0.2
+ # max_vel_scale_factor = 1.0
+ # max_acc_scale_factor = 0.2
+ # max_acc_scale_factor = 1.0
+ # move_group.set_max_velocity_scaling_factor(max_vel_scale_factor)
+ # move_group.set_max_acceleration_scaling_factor(max_acc_scale_factor)
+ # -- UNCOMMENT ABOVE TO SEE THE SCALING FACTOR IN ACTION --
+ move_group.set_joint_value_target(joint_goal)
+ move_group.plan()
+ move_group.go(wait=True)
+
+ # METHOD 2
+ # The go command can be called with joint values, poses, or without any
+ # parameters if you have already set the pose or joint target for the group.
+ move_group.go(joint_goal, wait=True)
+
+ # Calling ``stop()`` ensures that there is no residual movement
+ move_group.stop()
+
+ # # -- Plan pose goal --
+ pose_goal = Pose()
+ pose_goal.orientation.w = 1.0
+ pose_goal.position.x = 400
+ pose_goal.position.y = 100
+ pose_goal.position.z = 0.4
+
+ # move_group.set_pose_target(pose_goal)
+ retval, plan, _, error_code = move_group.plan(pose_goal)
+ plan = move_group.go(wait=True)
+
+ # Calling `stop()` ensures that there is no residual movement
+ move_group.stop()
+
+ # It is always good to clear your targets after planning with poses.
+ # Note: there is no equivalent function for clear_joint_value_targets()
+ move_group.clear_pose_targets()
# -- Plan hand goal --
# hand_move_group.set_joint_value_target([0.04, 0.04])
@@ -123,3 +124,10 @@
offset=0.0,
)
print("Added plane to scene")
+
+ # -- Get random ee pose --
+ # move_group.set_end_effector_link("panda_link8")
+ move_group.set_end_effector_link("panda_hand")
+ move_group.set_end_effector_link("panda_EE")
+ pose = move_group.get_random_pose()
+ print(f"Random pose: {pose}")
diff --git a/panda_gazebo/tests/manual/moveit_server_dynamic_reconfigure_test.py b/panda_gazebo/tests/manual/test_moveit_server_dynamic_reconfigure.py
similarity index 100%
rename from panda_gazebo/tests/manual/moveit_server_dynamic_reconfigure_test.py
rename to panda_gazebo/tests/manual/test_moveit_server_dynamic_reconfigure.py
diff --git a/panda_gazebo/tests/manual/panda_control_server_test.py b/panda_gazebo/tests/manual/test_panda_control_server.py
similarity index 100%
rename from panda_gazebo/tests/manual/panda_control_server_test.py
rename to panda_gazebo/tests/manual/test_panda_control_server.py
diff --git a/panda_gazebo/tests/manual/panda_control_switcher_test.py b/panda_gazebo/tests/manual/test_panda_control_switcher.py
similarity index 100%
rename from panda_gazebo/tests/manual/panda_control_switcher_test.py
rename to panda_gazebo/tests/manual/test_panda_control_switcher.py
diff --git a/panda_gazebo/tests/manual/panda_moveit_server_test.py b/panda_gazebo/tests/manual/test_panda_moveit_server.py
similarity index 100%
rename from panda_gazebo/tests/manual/panda_moveit_server_test.py
rename to panda_gazebo/tests/manual/test_panda_moveit_server.py