Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision
  • main
1 result

Target

Select target project
  • gut-teaching/introduction-to-robotics/introduction-to-robotics
  • florian.becker.dus/introduction-to-robotics
  • mohildenraaz/introduction-to-robotics
  • max.huthmacher/introduction-to-robotics
  • madhavapandiyan26/introduction-to-robotics
  • beckerst99/introduction-to-robotics
  • gohari_mojdeh/introduction-to-robotics
7 results
Select Git revision
  • main
1 result
Show changes
Show whitespace changes
Inline Side-by-side
Showing
with 1450 additions and 37 deletions
Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/warehouse.launch 0 → 100755
View file @ 1985345a
<launch>
<!-- The path to the database must be specified -->
<arg name="moveit_warehouse_database_path" />
<!-- Load warehouse parameters -->
<include file="$(dirname)/warehouse_settings.launch.xml" />
<!-- Run the DB server -->
<node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">
<param name="overwrite" value="false"/>
<param name="database_path" value="$(arg moveit_warehouse_database_path)" />
</node>
</launch>
Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/launch/warehouse_settings.launch.xml 0 → 100755
View file @ 1985345a
<launch>
<!-- Set the parameters for the warehouse and run the mongodb server. -->
<!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
<arg name="moveit_warehouse_port" default="33829" />
<!-- The default DB host for moveit -->
<arg name="moveit_warehouse_host" default="localhost" />
<!-- Set parameters for the warehouse -->
<param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
<param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
<param name="warehouse_exec" value="mongod" />
<param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />
</launch>
Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/package.xml 0 → 100755
View file @ 1985345a
<package>
<name>arm_robot_moveit</name>
<version>0.3.0</version>
<description>
An automatically generated package with all the configuration and launch files for using the arm_robot with the MoveIt Motion Planning Framework
</description>
<author email="abhirup.das@rwth-aachen.de">Abhirup Das</author>
<maintainer email="abhirup.das@rwth-aachen.de">Abhirup Das</maintainer>
<license>BSD</license>
<url type="website">http://moveit.ros.org/</url>
<url type="bugtracker">https://github.com/moveit/moveit/issues</url>
<url type="repository">https://github.com/moveit/moveit</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>moveit_fake_controller_manager</run_depend>
<run_depend>moveit_kinematics</run_depend>
<run_depend>moveit_planners</run_depend>
<run_depend>moveit_ros_visualization</run_depend>
<run_depend>moveit_setup_assistant</run_depend>
<run_depend>moveit_simple_controller_manager</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>joint_state_publisher_gui</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>rviz</run_depend>
<run_depend>tf2_ros</run_depend>
<run_depend>xacro</run_depend>
<!-- The next 2 packages are required for the gazebo simulation.
We don't include them by default to prevent installing gazebo and all its dependencies. -->
<!-- <run_depend>joint_trajectory_controller</run_depend> -->
<!-- <run_depend>gazebo_ros_control</run_depend> -->
<!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
<!-- <run_depend>warehouse_ros_mongo</run_depend> -->
<run_depend>arm_robot</run_depend>
</package>
\ No newline at end of file
Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/gettestpose.py 0 → 100755
View file @ 1985345a
#!/usr/bin/env python3
import sys
import rospy
import moveit_commander
import tf
from geometry_msgs.msg import Pose
def get_end_effector_pose():
# Initialize moveit_commander and rospy
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('get_end_effector_pose', anonymous=True)
# Initialize the robot, scene, and planning group
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm_group") # Replace "arm" with your robot's planning group
# Get the current pose of the end effector
end_effector_pose = group.get_current_pose().pose
# Extract quaternion from pose
quaternion = (
end_effector_pose.orientation.x,
end_effector_pose.orientation.y,
end_effector_pose.orientation.z,
end_effector_pose.orientation.w
)
# Convert quaternion to Euler angles (roll, pitch, yaw)
euler = tf.transformations.euler_from_quaternion(quaternion)
# Extract roll, pitch, yaw values
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
# Print out the result
rospy.loginfo("End Effector Pose: \nPosition: {}\nOrientation (Euler Angles): Roll: {}, Pitch: {}, Yaw: {}".format(
end_effector_pose.position, roll, pitch, yaw))
# Close moveit_commander
moveit_commander.roscpp_shutdown()
if __name__ == "__main__":
get_end_effector_pose()
Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/pause_resume_service.py 0 → 100755
View file @ 1985345a
#! /usr/bin/python3
from std_srvs.srv import SetBool, SetBoolResponse # Import the correct response
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
# Function to pause the execution
def pause_execution(request):
move_group.stop() # Stop the robot's movement
return SetBoolResponse(success=True, message="Execution Paused")
# Function to resume the execution
def resume_execution(request):
target_pose = move_group.get_current_pose().pose # Use current pose or set a new one
move_group.set_pose_target(target_pose)
move_group.go(wait=True) # Resume the robot's movement
return SetBoolResponse(success=True, message="Execution Resumed")
def main():
rospy.init_node('pause_resume_execution_service')
# Initialize the MoveGroupCommander for the robot's planning group (e.g., 'arm')
global move_group
move_group = MoveGroupCommander("arm_group")
# Define the custom services for pausing and resuming execution
pause_service = rospy.Service('/pause_execution', SetBool, pause_execution)
resume_service = rospy.Service('/resume_execution', SetBool, resume_execution)
rospy.loginfo("Services are running...")
rospy.spin()
if __name__ == "__main__":
main()
Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/pick_place_fk.py 0 → 100755
View file @ 1985345a
#! /usr/bin/python3
import rospy
import sys
import math
import actionlib
import moveit_commander
from moveit_msgs.msg import CollisionObject, ExecuteTrajectoryAction, ExecuteTrajectoryGoal
import geometry_msgs.msg
from shape_msgs.msg import SolidPrimitive
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
class ArmRobot:
def __init__(self, Group_Name, Grasping_Group_Name):
'''
Function to Intialize the Node
'''
rospy.loginfo("--- Starting initialising a robotics node ---")
#initialize moveit Commander
self._commander = moveit_commander.roscpp_initialize(sys.argv)
#RobotCommander Initialize-Outerlevel Interface to Robot
self._robot = moveit_commander.RobotCommander()
#instantiate planning scene
self._scene = moveit_commander.PlanningSceneInterface()
#defining move groups
self._planning_group = Group_Name
self._grasping_group = Grasping_Group_Name
#Instantiate MoveGroupCommander Object
self._group = moveit_commander.MoveGroupCommander(self._planning_group)
self._eef_group = moveit_commander.MoveGroupCommander(self._grasping_group)
#Get the planning frame, end effector link and the robot group names
self._planning_frame = self._group.get_planning_frame()
self._eef_link = self._group.get_end_effector_link()
self._group_names = self._robot.get_group_names()
self._group.set_goal_position_tolerance(1E-2)
self._group.set_goal_orientation_tolerance(1E-3)
self._group.set_planning_time(15) #setting planning time in seconds
self._execute_trajectory_client = actionlib.SimpleActionClient('execute_trajectory', ExecuteTrajectoryAction)
self._execute_trajectory_client.wait_for_server()
#print the info
#here the '\033[95m' represents the standard colour "LightMagenta" in terminals. For details, refer: https://pkg.go.dev/github.com/whitedevops/colors
#The '\033[0m' is added at the end of string to reset the terminal colours to default
rospy.loginfo('\033[95m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m')
rospy.loginfo('\033[95m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m')
rospy.loginfo('\033[95m' + "Group Names: {}".format(self._group_names) + '\033[0m')
rospy.loginfo('\033[95m' + " >>> ArmRobot initialization is done." + '\033[0m')
def wait_for_state_update(self,obj_id,obj_is_known=False, obj_is_attached=False, timeout=2):
scene = self._scene
start_time = rospy.get_time()
secondary_time = rospy.get_time()
while(secondary_time-start_time < timeout) and not rospy.is_shutdown():
attached_objects = scene.get_attached_objects([obj_id])
is_attached = len(attached_objects.keys()) > 0
#test
is_known = obj_id in scene.get_known_object_names()
if (obj_is_attached == is_attached) and (obj_is_known == is_known):
return True
#sleep to promote other threads
rospy.sleep(0.1)
secondary_time = rospy.get_time()
#if while loop exited without returning
return False
def create_object(self,obj_id,pose,ref_frame, dims, timeout=2):
object = CollisionObject() #object variable type
object.id = obj_id
object.header.frame_id = ref_frame
solid = SolidPrimitive()
solid.type = solid.BOX
solid.dimensions = dims
object.primitives = [solid]
scene = self._scene
## First, create an message object of type pose to deifne box positions
object_pose = geometry_msgs.msg.Pose()
#box_pose.header.frame_id=self._armrobot.get_planning_frame()
#object_pose.header.frame_id = ref_frame
#set the position massage arguments
object_pose.position.x = pose[0]
object_pose.position.y = pose[1]
object_pose.position.z = pose[2]
#creating quaternion from (rpy)
orientation_ = quaternion_from_euler(pose[3], pose[4], pose[5])
object_pose.orientation.x = orientation_[0]
object_pose.orientation.y = orientation_[1]
object_pose.orientation.x = orientation_[2]
object_pose.orientation.w = orientation_[3]
#Adding Object
object.primitive_poses = [object_pose]
object.operation = object.ADD
#Add the box in the scene
scene.add_object(object)
#https://docs.ros.org/en/noetic/api/moveit_commander/html/classmoveit__commander_1_1planning__scene__interface_1_1PlanningSceneInterface.html
#https://docs.ros.org/en/noetic/api/moveit_commander/html/planning__scene__interface_8py_source.html
#Check if the box is added sucessfully
return self.wait_for_state_update(obj_id, obj_is_known=True, timeout=timeout)
def gripper_action(self,gripper, action="open"):
'''
Function to execute the required action of the gripper.
Args:
gripper: group_name of gripper in the manipulator
action: action to perform. Allowed values ["open", "close"]
Returns:
None.
'''
print("Executing gripper action = ", action)
if(action == "open"):
self.set_pose(gripper,"gripper_open")
elif(action == "close"):
self.set_pose(gripper,"gripper_close")
else:
print("Action undefined")
def set_pose(self,group,arg_pose_name):
group.set_named_target(arg_pose_name)
plan_success, plan, planning_time, error_code = group.plan()
goal=ExecuteTrajectoryGoal()
goal.trajectory = plan
self._execute_trajectory_client.send_goal(goal)
self._execute_trajectory_client.wait_for_result()
rospy.loginfo('\033[32m' + "Now at: {}".format(arg_pose_name) + '\033[0m')
def pick_action(self,scene, robot, arm, gripper,box):
# Gripper open
self.gripper_action(gripper, action="open")
# Sleep
rospy.sleep(0.5)
# 1) Move to pre-pick location
self.set_pose(arm,"pre_pick_pose_1")
# Sleep
rospy.sleep(0.5)
# 2) Move to pick location
self.set_pose(arm,"pick_pose_1")
# Sleep
rospy.sleep(0.5)
# 3) Close the gripper and attach the object
# Close gripper
self.gripper_action(gripper, action="close")
# Attaching the object
eef_frame = arm.get_end_effector_link()
grasping_group = self._grasping_group
touch_links= robot.get_link_names(group=grasping_group)
scene.attach_box(eef_frame, box, touch_links=touch_links)
# Sleep
rospy.sleep(0.5)
# 4) Move to post-pick location
# Define post-pick pose
post_pick_pose = "pre_pick_pose_1"
# Move
self.set_pose(arm, post_pick_pose)
# Sleep
rospy.sleep(0.5)
return None
# Object Place function
def place_action(self, scene, arm, gripper, box):
# 1) Move to pre-drop location
# Move
pre_place_pose = "pre_place_pose"
self.set_pose(arm, pre_place_pose)
# Sleep
rospy.sleep(0.5)
# 2) Move to drop location
place_pose = "place_pose"
self.set_pose(arm, place_pose)
# Sleep
rospy.sleep(0.5)
# 3) Open the gripper and detach the object
# Open gripper
self.gripper_action(gripper, action="open")
# Detaching the object
eef_frame = arm.get_end_effector_link()
scene.remove_attached_object(eef_frame, name=box)
# Sleep
rospy.sleep(0.5)
# 4) Move to post-drop location
# Define post-drop pose
post_place_pose="pre_place_pose"
# Move
self.set_pose(arm, post_place_pose)
# Sleep
rospy.sleep(0.5)
return None
#==================================================================
def pick_action_reverse(self,scene, robot, arm, gripper,box):
# Gripper open
self.gripper_action(gripper, action="open")
# Sleep
rospy.sleep(0.5)
# 1) Move to pre-pick location
self.set_pose(arm,"pre_place_pose")
# Sleep
rospy.sleep(0.5)
# 2) Move to pick location
self.set_pose(arm,"place_pose")
# Sleep
rospy.sleep(0.5)
# 3) Close the gripper and attach the object
# Close gripper
self.gripper_action(gripper, action="close")
# Attaching the object
eef_frame = arm.get_end_effector_link()
grasping_group = self._grasping_group
touch_links= robot.get_link_names(group=grasping_group)
scene.attach_box(eef_frame, box, touch_links=touch_links)
# Sleep
rospy.sleep(0.5)
# 4) Move to post-pick location
# Define post-pick pose
post_pick_pose = "pre_place_pose"
# Move
self.set_pose(arm, post_pick_pose)
# Sleep
rospy.sleep(0.5)
return None
# Object Place function
def place_action_reverse(self, scene, arm, gripper, box):
# 1) Move to pre-drop location
# Move
pre_place_pose = "pre_pick_pose_1"
self.set_pose(arm, pre_place_pose)
# Sleep
rospy.sleep(0.5)
# 2) Move to drop location
place_pose = "pick_pose_1"
self.set_pose(arm, place_pose)
# Sleep
rospy.sleep(0.5)
# 3) Open the gripper and detach the object
# Open gripper
self.gripper_action(gripper, action="open")
# Detaching the object
eef_frame = arm.get_end_effector_link()
scene.remove_attached_object(eef_frame, name=box)
# Sleep
rospy.sleep(0.5)
# 4) Move to post-drop location
# Define post-drop pose
post_place_pose="pre_pick_pose_1"
# Move
self.set_pose(arm, post_place_pose)
# Sleep
rospy.sleep(0.5)
return None
#==================================================================
# Destructor
def __del__(self):
moveit_commander.roscpp_shutdown()
rospy.loginfo('\033[94m' + "Object of class ArmRobot Deleted." + '\033[0m')
def main():
#Initialize a ROS Node
rospy.init_node('add_attach_detach_objects_in_Rviz', anonymous=True)
robotArm = ArmRobot("arm_group", "gripper_group")
robotArm._scene.remove_world_object()
rospy.loginfo("-- Adding Objects --")
# robotArm.create_object(obj_id="ceiling", ref_frame=robotArm._planning_frame,pose=[0.0,0,0.30,0.0,0.0,0.0],dims=[0.4,0.4,0.01])
robotArm.create_object(obj_id="table1", ref_frame=robotArm._planning_frame,pose=[0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])
robotArm.create_object(obj_id="table2", ref_frame=robotArm._planning_frame,pose=[-0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])
robotArm.create_object(obj_id="box1", ref_frame=robotArm._planning_frame,pose=[0.2635,-0.0275,0.0300,0.0,0.0,0.0],dims=[0.04,0.04,0.04])
#robotArm.add_box("package$2",0.00,0.6318,0.015,0.03,0.03,0.03)
rospy.loginfo("Picking object 1")
#robotArm.pick_action(robotArm._scene, robotArm._robot, robotArm._group, robotArm._eef_group,box="box1")
# robotArm.set_pose(robotArm._group,"arm_home")
# Place box1
#rospy.loginfo("Placing object 1")
#robotArm.place_action(robotArm._scene, robotArm._group, robotArm._eef_group, box="box1")
#==================================================================
for i in range(5):
robotArm.pick_action(robotArm._scene, robotArm._robot, robotArm._group, robotArm._eef_group,box="box1")
rospy.loginfo("Placing object 1")
robotArm.place_action(robotArm._scene, robotArm._group, robotArm._eef_group, box="box1")
robotArm.pick_action_reverse(robotArm._scene, robotArm._robot, robotArm._group, robotArm._eef_group,box="box1")
rospy.loginfo("Placing object 1")
robotArm.place_action_reverse(robotArm._scene, robotArm._group, robotArm._eef_group, box="box1")
#==================================================================
robotArm.set_pose(robotArm._group,"arm_home")
robotArm.set_pose(robotArm._eef_group,"gripper_close")
robotArm._scene.remove_world_object()
if __name__ == '__main__':
main()
Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/pick_place_ik.py 0 → 100755
View file @ 1985345a
#! /usr/bin/python3
import rospy
import sys
import math
import actionlib
import moveit_commander
from moveit_msgs.msg import CollisionObject, ExecuteTrajectoryAction, ExecuteTrajectoryGoal
import geometry_msgs.msg
from shape_msgs.msg import SolidPrimitive
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
class ArmRobot:
def __init__(self, Group_Name, Grasping_Group_Name):
'''
Function to Intialize the Node
'''
rospy.loginfo("--- Starting initialising a robotics node ---")
#initialize moveit Commander
self._commander = moveit_commander.roscpp_initialize(sys.argv)
#RobotCommander Initialize-Outerlevel Interface to Robot
self._robot = moveit_commander.RobotCommander()
#instantiate planning scene
self._scene = moveit_commander.PlanningSceneInterface()
#defining move groups
self._planning_group = Group_Name
self._grasping_group = Grasping_Group_Name
#Instantiate MoveGroupCommander Object
self._group = moveit_commander.MoveGroupCommander(self._planning_group)
self._eef_group = moveit_commander.MoveGroupCommander(self._grasping_group)
#Get the planning frame, end effector link and the robot group names
self._planning_frame = self._group.get_planning_frame()
self._eef_link = self._group.get_end_effector_link()
self._group_names = self._robot.get_group_names()
self._group.set_goal_position_tolerance(1E-2)
self._group.set_goal_orientation_tolerance(1E-3)
self._group.set_planning_time(15) #setting planning time in seconds
self._execute_trajectory_client = actionlib.SimpleActionClient('execute_trajectory', ExecuteTrajectoryAction)
self._execute_trajectory_client.wait_for_server()
#print the info
#here the '\033[95m' represents the standard colour "LightMagenta" in terminals. For details, refer: https://pkg.go.dev/github.com/whitedevops/colors
#The '\033[0m' is added at the end of string to reset the terminal colours to default
rospy.loginfo('\033[95m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m')
rospy.loginfo('\033[95m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m')
rospy.loginfo('\033[95m' + "Group Names: {}".format(self._group_names) + '\033[0m')
rospy.loginfo('\033[95m' + " >>> ArmRobot initialization is done." + '\033[0m')
def wait_for_state_update(self,obj_id,obj_is_known=False, obj_is_attached=False, timeout=2):
scene = self._scene
start_time = rospy.get_time()
secondary_time = rospy.get_time()
while(secondary_time-start_time < timeout) and not rospy.is_shutdown():
attached_objects = scene.get_attached_objects([obj_id])
is_attached = len(attached_objects.keys()) > 0
#test
is_known = obj_id in scene.get_known_object_names()
if (obj_is_attached == is_attached) and (obj_is_known == is_known):
return True
#sleep to promote other threads
rospy.sleep(0.1)
secondary_time = rospy.get_time()
#if while loop exited without returning
return False
def create_object(self,obj_id,pose,ref_frame, dims, timeout=2):
object = CollisionObject() #object variable type
object.id = obj_id
object.header.frame_id = ref_frame
solid = SolidPrimitive()
solid.type = solid.BOX
solid.dimensions = dims
object.primitives = [solid]
scene = self._scene
## First, create an message object of type pose to deifne box positions
object_pose = geometry_msgs.msg.Pose()
#box_pose.header.frame_id=self._armrobot.get_planning_frame()
#object_pose.header.frame_id = ref_frame
#set the position massage arguments
object_pose.position.x = pose[0]
object_pose.position.y = pose[1]
object_pose.position.z = pose[2]
#creating quaternion from (rpy)
orientation_ = quaternion_from_euler(pose[3], pose[4], pose[5])
object_pose.orientation.x = orientation_[0]
object_pose.orientation.y = orientation_[1]
object_pose.orientation.x = orientation_[2]
object_pose.orientation.w = orientation_[3]
#Adding Object
object.primitive_poses = [object_pose]
object.operation = object.ADD
#Add the box in the scene
scene.add_object(object)
#https://docs.ros.org/en/noetic/api/moveit_commander/html/classmoveit__commander_1_1planning__scene__interface_1_1PlanningSceneInterface.html
#https://docs.ros.org/en/noetic/api/moveit_commander/html/planning__scene__interface_8py_source.html
#Check if the box is added sucessfully
return self.wait_for_state_update(obj_id, obj_is_known=True, timeout=timeout)
def gripper_action(self,gripper, action="open"):
'''
Function to execute the required action of the gripper.
Args:
gripper: group_name of gripper in the manipulator
action: action to perform. Allowed values ["open", "close"]
Returns:
None.
'''
print("Executing gripper action = ", action)
if(action == "open"):
self.set_pose(gripper,"gripper_open")
elif(action == "close"):
self.set_pose(gripper,"gripper_close")
else:
print("Action undefined")
def set_pose(self,eef,arg_pose_name):
eef.set_named_target(arg_pose_name)
plan_success, plan, planning_time, error_code = eef.plan()
goal=ExecuteTrajectoryGoal()
goal.trajectory = plan
self._execute_trajectory_client.send_goal(goal)
self._execute_trajectory_client.wait_for_result()
rospy.loginfo('\033[32m' + "Now at: {}".format(arg_pose_name) + '\033[0m')
def move_to_pose(self,arm, goal_pose, angles):
'''
Function to move the robot arm to the requested pose
Args:
arm: robot's arm group that should be moved.
goal_pose: requested pose (in the planning frame)
Returns:
None
'''
#NOTE:
#The planner will create a plan for the robot to reach
#the goal pose.
rospy.logwarn("planning and moving to location")
gripper_angle = geometry_msgs.msg.Quaternion()
#Orientation of the tcp(panda_hand_tcp) w.r.t base frame(panda_link0)
quaternion = quaternion_from_euler(angles[0],angles[1],angles[2])
gripper_angle.x = quaternion[0]
gripper_angle.y = quaternion[1]
gripper_angle.z = quaternion[2]
gripper_angle.w = quaternion[3]
#defining pre-grasp position
grasp_pose = geometry_msgs.msg.Pose()
grasp_pose.position.x = goal_pose.position.x
grasp_pose.position.y = goal_pose.position.y
grasp_pose.position.z = goal_pose.position.z
grasp_pose.orientation = gripper_angle
print("Exectuting move_to_pose ({} , {}, {})"
.format(grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z))
arm.set_goal_position_tolerance(0.01) #setting tolerence for pose
arm.set_goal_orientation_tolerance(0.1)
arm.set_pose_target(grasp_pose) #setting the pose of the end-effector
plan = arm.go(wait=True) #move the arm to the grasp_pose.
arm.stop() #ensures that there is no residual movement.
arm.clear_pose_targets() #to clear the existing targets.
def pick_action(self,scene, robot, arm, gripper, pick_pose, box):
# Gripper open
self.gripper_action(gripper, action="open")
# Sleep
rospy.sleep(0.5)
# 1) Move to pre-pick location
pre_pick_angle = [-1.5716,0.0, -0.1044]
#Sub 0.036 to x, add 0.004 to y and 0.170 to z for prepick
# Define pre-pick pose
pre_pick_pose = geometry_msgs.msg.Pose()
pre_pick_pose.position.x = pick_pose.position.x - 0.036
pre_pick_pose.position.y = pick_pose.position.y + 0.004
pre_pick_pose.position.z = pick_pose.position.z + 0.2
# Move
pick_angle = [-1.5716,0.7727, -0.1050]
self.move_to_pose(arm, pre_pick_pose, pre_pick_angle)
# Sleep
rospy.sleep(0.5)
# 2) Move to pick location
self.move_to_pose(arm, pick_pose,pick_angle)
# Sleep
rospy.sleep(0.5)
# 3) Close the gripper and attach the object
# Close gripper
self.gripper_action(gripper, action="close")
# Attaching the object
eef_frame = arm.get_end_effector_link()
grasping_group = self._grasping_group
touch_links= robot.get_link_names(group=grasping_group)
scene.attach_box(eef_frame, box, touch_links=touch_links)
# Sleep
rospy.sleep(0.5)
# 4) Move to post-pick location
# Define post-pick pose
post_pick_pose = pre_pick_pose
post_pick_angle = pre_pick_angle
# Move
self.move_to_pose(arm, post_pick_pose, post_pick_angle)
# Sleep
rospy.sleep(0.5)
return None
# Object Place function
def place_action(self, scene, arm, gripper, place_pose, box):
# 1) Move to pre-drop location
# Define pre-drop pose
pre_place_angle = [-1.5611, 0.0784,3.0379]
pre_place_pose = geometry_msgs.msg.Pose()
pre_place_pose.position.x = place_pose.position.x + 0.0363
pre_place_pose.position.y = place_pose.position.y - 0.0044
pre_place_pose.position.z = place_pose.position.z + 0.2
# Move
self.move_to_pose(arm, pre_place_pose, pre_place_angle)
# Sleep
rospy.sleep(0.5)
# 2) Move to drop location
place_angle = [-1.5716,0.7727, -0.1050]
self.move_to_pose(arm, place_pose,place_angle)
# Sleep
rospy.sleep(0.5)
# 3) Open the gripper and detach the object
# Open gripper
self.gripper_action(gripper, action="open")
# Detaching the object
eef_frame = arm.get_end_effector_link()
scene.remove_attached_object(eef_frame, name=box)
# Sleep
rospy.sleep(0.5)
# 4) Move to post-drop location
# Define post-drop pose
post_place_pose=pre_place_pose
post_place_angle=pre_place_angle
# Move
self.move_to_pose(arm, post_place_pose,post_place_angle)
# Sleep
rospy.sleep(0.5)
return None
# Destructor
def __del__(self):
moveit_commander.roscpp_shutdown()
rospy.loginfo('\033[94m' + "Object of class ArmRobot Deleted." + '\033[0m')
def main():
#Initialize a ROS Node
rospy.init_node('add_attach_detach_objects_in_Rviz', anonymous=True)
robotArm = ArmRobot("arm_group", "gripper_group")
robotArm._scene.remove_world_object()
rospy.loginfo("-- Adding Objects --")
robotArm.create_object(obj_id="ceiling", ref_frame=robotArm._planning_frame,pose=[0.0,0,0.30,0.0,0.0,0.0],dims=[0.4,0.4,0.01])
# robotArm.create_object(obj_id="table1", ref_frame=robotArm._planning_frame,pose=[0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])
# robotArm.create_object(obj_id="table2", ref_frame=robotArm._planning_frame,pose=[-0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])
robotArm.create_object(obj_id="box1", ref_frame=robotArm._planning_frame,pose=[0.2635,-0.0275,0.0300,0.0,0.0,0.0],dims=[0.04,0.04,0.04])
#robotArm.add_box("package$2",0.00,0.6318,0.015,0.03,0.03,0.03)
rospy.loginfo("Picking object 1")
pick_pose_1 = geometry_msgs.msg.Pose()
pick_pose_1.position.x = 0.2635
pick_pose_1.position.y = -0.0275
pick_pose_1.position.z = 0.0300
# pick_pose_1=robotArm._scene.getObject("box1").primitive_poses[0]
# rospy.loginfo(fpick_pose_1.position.x, pick_pose_1.position.y, pick_pose_1.position.z)
robotArm.pick_action(robotArm._scene, robotArm._robot, robotArm._group, robotArm._eef_group, pick_pose_1, box="box1")
robotArm.set_pose(robotArm._group,"arm_home")
# Place box1
rospy.loginfo("Placing object 1")
place_pose_1 = geometry_msgs.msg.Pose()
place_pose_1.position.x = -0.2635
place_pose_1.position.y = 0.0275
place_pose_1.position.z = 0.0300
robotArm.place_action(robotArm._scene, robotArm._group, robotArm._eef_group, place_pose_1, box="box1")
robotArm.set_pose(robotArm._group,"arm_home")
robotArm.set_pose(robotArm._eef_group,"gripper_close")
robotArm._scene.remove_world_object()
if __name__ == '__main__':
main()
Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/pick_place_ik_copy.py 0 → 100755
View file @ 1985345a
#! /usr/bin/python3
import rospy
import sys
import math
import actionlib
import moveit_commander
from moveit_msgs.msg import CollisionObject, ExecuteTrajectoryAction, ExecuteTrajectoryGoal
import geometry_msgs.msg
from shape_msgs.msg import SolidPrimitive
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
import trac_ik_python.trac_ik as tracik
class ArmRobot:
def __init__(self, Group_Name, Grasping_Group_Name):
'''
Function to Intialize the Node
'''
rospy.loginfo("--- Starting initialising a robotics node ---")
#initialize moveit Commander
self._commander = moveit_commander.roscpp_initialize(sys.argv)
#RobotCommander Initialize-Outerlevel Interface to Robot
self._robot = moveit_commander.RobotCommander()
#instantiate planning scene
self._scene = moveit_commander.PlanningSceneInterface()
#defining move groups
self._planning_group = Group_Name
self._grasping_group = Grasping_Group_Name
#Instantiate MoveGroupCommander Object
self._group = moveit_commander.MoveGroupCommander(self._planning_group)
self._eef_group = moveit_commander.MoveGroupCommander(self._grasping_group)
#Get the planning frame, end effector link and the robot group names
self._planning_frame = self._group.get_planning_frame()
self._eef_link = self._group.get_end_effector_link()
self._group_names = self._robot.get_group_names()
self._group.set_goal_position_tolerance(1E-2)
self._group.set_goal_orientation_tolerance(1E-3)
self._group.set_planning_time(15) #setting planning time in seconds
self._execute_trajectory_client = actionlib.SimpleActionClient('execute_trajectory', ExecuteTrajectoryAction)
self._execute_trajectory_client.wait_for_server()
#print the info
#here the '\033[95m' represents the standard colour "LightMagenta" in terminals. For details, refer: https://pkg.go.dev/github.com/whitedevops/colors
#The '\033[0m' is added at the end of string to reset the terminal colours to default
rospy.loginfo('\033[95m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m')
rospy.loginfo('\033[95m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m')
rospy.loginfo('\033[95m' + "Group Names: {}".format(self._group_names) + '\033[0m')
rospy.loginfo('\033[95m' + " >>> ArmRobot initialization is done." + '\033[0m')
def wait_for_state_update(self,obj_id,obj_is_known=False, obj_is_attached=False, timeout=4):
scene = self._scene
start_time = rospy.get_time()
secondary_time = rospy.get_time()
while(secondary_time-start_time < timeout) and not rospy.is_shutdown():
attached_objects = scene.get_attached_objects([obj_id])
is_attached = len(attached_objects.keys()) > 0
#test
is_known = obj_id in scene.get_known_object_names()
if (obj_is_attached == is_attached) and (obj_is_known == is_known):
return True
#sleep to promote other threads
rospy.sleep(0.1)
secondary_time = rospy.get_time()
#if while loop exited without returning
return False
def create_object(self,obj_id,pose,ref_frame, dims, timeout=4):
object = CollisionObject() #object variable type
object.id = obj_id
object.header.frame_id = ref_frame
solid = SolidPrimitive()
solid.type = solid.BOX
solid.dimensions = dims
object.primitives = [solid]
scene = self._scene
## First, create an message object of type pose to deifne box positions
object_pose = geometry_msgs.msg.Pose()
#box_pose.header.frame_id=self._armrobot.get_planning_frame()
#object_pose.header.frame_id = ref_frame
#set the position massage arguments
object_pose.position.x = pose[0]
object_pose.position.y = pose[1]
object_pose.position.z = pose[2]
#creating quaternion from (rpy)
orientation_ = quaternion_from_euler(pose[3], pose[4], pose[5])
object_pose.orientation.x = orientation_[0]
object_pose.orientation.y = orientation_[1]
object_pose.orientation.x = orientation_[2]
object_pose.orientation.w = orientation_[3]
#Adding Object
object.primitive_poses = [object_pose]
object.operation = object.ADD
#Add the box in the scene
scene.add_object(object)
#https://docs.ros.org/en/noetic/api/moveit_commander/html/classmoveit__commander_1_1planning__scene__interface_1_1PlanningSceneInterface.html
#https://docs.ros.org/en/noetic/api/moveit_commander/html/planning__scene__interface_8py_source.html
#Check if the box is added sucessfully
return self.wait_for_state_update(obj_id, obj_is_known=True, timeout=timeout)
def gripper_action(self,gripper, action="open"):
'''
Function to execute the required action of the gripper.
Args:
gripper: group_name of gripper in the manipulator
action: action to perform. Allowed values ["open", "close"]
Returns:
None.
'''
print("Executing gripper action = ", action)
if(action == "open"):
self.set_pose(gripper,"gripper_open")
elif(action == "close"):
self.set_pose(gripper,"gripper_close")
else:
print("Action undefined")
def set_pose(self,eef,arg_pose_name):
eef.set_named_target(arg_pose_name)
plan_success, plan, planning_time, error_code = eef.plan()
goal=ExecuteTrajectoryGoal()
goal.trajectory = plan
self._execute_trajectory_client.send_goal(goal)
self._execute_trajectory_client.wait_for_result()
rospy.loginfo('\033[32m' + "Now at: {}".format(arg_pose_name) + '\033[0m')
def move_to_pose_or_joint(self, arm, goal_pose=None, joint_angles=None, angles=None):
'''
Function to move the robot arm either to a requested pose (Cartesian) or joint positions (joint-space)
Args:
arm: the robot arm group that should be moved.
goal_pose: requested pose (in the planning frame) (optional)
joint_angles: joint angles (list) to move to (optional)
angles: orientation angles for pose (optional)
Returns:
None
'''
if joint_angles: # If joint_angles are provided, move to joint target
arm.set_joint_value_target(joint_angles)
success = arm.go(wait=True)
if success:
rospy.loginfo("Move to approximate joint angles successful.")
arm.stop() #ensures that there is no residual movement.
arm.clear_pose_targets()
else:
rospy.logwarn("Move to approximate joint angles failed.")
elif goal_pose: # Otherwise, move to pose (Cartesian)
rospy.logwarn("Planning and moving to location (Cartesian)")
# Convert Euler angles to quaternion
quaternion = quaternion_from_euler(angles[0], angles[1], angles[2])
gripper_angle = geometry_msgs.msg.Quaternion()
gripper_angle.x = quaternion[0]
gripper_angle.y = quaternion[1]
gripper_angle.z = quaternion[2]
gripper_angle.w = quaternion[3]
# Define the target pose
grasp_pose = geometry_msgs.msg.Pose()
grasp_pose.position.x = goal_pose.position.x
grasp_pose.position.y = goal_pose.position.y
grasp_pose.position.z = goal_pose.position.z
grasp_pose.orientation = gripper_angle
rospy.loginfo(f"Executing move_to_pose with position: {grasp_pose.position.x}, {grasp_pose.position.y}, {grasp_pose.position.z}")
ik_solver = tracik.TrackerIK("base_link", "link4", "trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin")
# Use inverse kinematics to find the joint angles for the target pose
ik_successful, joint_angles = ik_solver.get_ik(grasp_pose)
if ik_successful:
rospy.loginfo(f"IK found joint angles: {joint_angles}")
arm.set_joint_value_target(joint_angles)
success = arm.go(wait=True)
if success:
rospy.loginfo("Move to approximate joint angles successful.")
arm.stop() #ensures that there is no residual movement.
arm.clear_pose_targets()
else:
rospy.logwarn("Move to approximate joint angles failed.")
else:
rospy.logwarn("Inverse Kinematics failed to find a solution.")
def move_to_pose(self,arm, goal_pose, angles):
'''
Function to move the robot arm to the requested pose
Args:
arm: robot's arm group that should be moved.
goal_pose: requested pose (in the planning frame)
Returns:
None
'''
#NOTE:
#The planner will create a plan for the robot to reach
#the goal pose.
rospy.logwarn("planning and moving to location")
gripper_angle = geometry_msgs.msg.Quaternion()
#Orientation of the tcp(panda_hand_tcp) w.r.t base frame(panda_link0)
quaternion = quaternion_from_euler(angles[0],angles[1],angles[2])
gripper_angle.x = quaternion[0]
gripper_angle.y = quaternion[1]
gripper_angle.z = quaternion[2]
gripper_angle.w = quaternion[3]
#defining pre-grasp position
grasp_pose = geometry_msgs.msg.Pose()
grasp_pose.position.x = goal_pose.position.x
grasp_pose.position.y = goal_pose.position.y
grasp_pose.position.z = goal_pose.position.z
grasp_pose.orientation = gripper_angle
print("Exectuting move_to_pose ({} , {}, {})"
.format(grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z))
arm.set_goal_position_tolerance(0.01) #setting tolerence for pose
arm.set_goal_orientation_tolerance(0.1)
arm.set_pose_target(grasp_pose) #setting the pose of the end-effector
plan = arm.go(wait=True) #move the arm to the grasp_pose.
arm.stop() #ensures that there is no residual movement.
arm.clear_pose_targets() #to clear the existing targets.
def pick_action(self,scene, robot, arm, gripper, pick_pose, box):
# Gripper open
self.gripper_action(gripper, action="open")
# Sleep
rospy.sleep(1)
# 1) Move to pre-pick location
pre_pick_angle = [-1.5716,0.0, -0.1044]
#Sub 0.036 to x, add 0.004 to y and 0.170 to z for prepick
# Define pre-pick pose
pre_pick_pose = geometry_msgs.msg.Pose()
pre_pick_pose.position.x = pick_pose.position.x - 0.036
pre_pick_pose.position.y = pick_pose.position.y + 0.004
pre_pick_pose.position.z = pick_pose.position.z + 0.2
# Move
self.move_to_pose_or_joint(arm, pre_pick_pose, angles=pre_pick_angle)
# Sleep
rospy.sleep(1)
# 2) Move to pick location
pick_angle = [-1.5716,0.7727, -0.1050]
self.move_to_pose_or_joint(arm, pick_pose,angles=pick_angle)
# Sleep
rospy.sleep(1)
# 3) Close the gripper and attach the object
# Close gripper
self.gripper_action(gripper, action="close")
# Attaching the object
eef_frame = arm.get_end_effector_link()
grasping_group = self._grasping_group
touch_links= robot.get_link_names(group=grasping_group)
scene.attach_box(eef_frame, box, touch_links=touch_links)
# Sleep
rospy.sleep(1)
# 4) Move to post-pick location
# Define post-pick pose
post_pick_pose = pre_pick_pose
post_pick_angle = pre_pick_angle
# Move
self.move_to_pose_or_joint(arm, post_pick_pose, angles=post_pick_angle)
# Sleep
rospy.sleep(1)
return None
# Object Place function
def place_action(self, scene, arm, gripper, place_pose, box):
# #INTERMEDIATE
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.0248 # Example x position
target_pose.position.y = 0.2273 # Example y position
target_pose.position.z = 0.2330 # Example z position
target_angle=[-1.5710, -0.0002, 1.4621]
self.move_to_pose_or_joint(arm, target_pose,angles=target_angle)
# 1) Move to pre-drop location
# Define pre-drop pose
pre_place_angle = [-1.5611, 0.0784,3.0379]
pre_place_pose = geometry_msgs.msg.Pose()
pre_place_pose.position.x = place_pose.position.x + 0.0363
pre_place_pose.position.y = place_pose.position.y - 0.0044
pre_place_pose.position.z = place_pose.position.z + 0.2
# Move
self.move_to_pose_or_joint(arm, pre_place_pose, angles=pre_place_angle)
# Sleep
rospy.sleep(1)
# 2) Move to drop location
place_angle = [-1.5716,0.7727, -0.1050]
self.move_to_pose_or_joint(arm, place_pose,angles=place_angle)
# Sleep
rospy.sleep(1)
# 3) Open the gripper and detach the object
# Open gripper
self.gripper_action(gripper, action="open")
# Detaching the object
eef_frame = arm.get_end_effector_link()
scene.remove_attached_object(eef_frame, name=box)
# Sleep
rospy.sleep(1)
# 4) Move to post-drop location
# Define post-drop pose
post_place_pose=pre_place_pose
post_place_angle=pre_place_angle
# Move
self.move_to_pose_or_joint(arm, post_place_pose,angles=post_place_angle)
# #INTERMEDIATE
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.0248 # Example x position
target_pose.position.y = 0.2273 # Example y position
target_pose.position.z = 0.2330 # Example z position
target_angle=[-1.5710, -0.0002, 1.4621]
self.move_to_pose_or_joint(arm, target_pose,angles=target_angle)
self.gripper_action(gripper, action="close")
# Sleep
rospy.sleep(1)
return None
# Destructor
def __del__(self):
moveit_commander.roscpp_shutdown()
rospy.loginfo('\033[94m' + "Object of class ArmRobot Deleted." + '\033[0m')
def main():
#Initialize a ROS Node
rospy.init_node('add_attach_detach_objects_in_Rviz', anonymous=True)
robotArm = ArmRobot("arm_group", "gripper_group")
robotArm._scene.remove_world_object()
rospy.loginfo("-- Adding Objects --")
robotArm.create_object(obj_id="ceiling", ref_frame=robotArm._planning_frame,pose=[0.0,0,0.30,0.0,0.0,0.0],dims=[0.4,0.4,0.01])
# robotArm.create_object(obj_id="table1", ref_frame=robotArm._planning_frame,pose=[0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])
# robotArm.create_object(obj_id="table2", ref_frame=robotArm._planning_frame,pose=[-0.2,0,-0.01,0.0,0.0,0.0],dims=[0.2,0.5,0.01])
robotArm.create_object(obj_id="box1", ref_frame=robotArm._planning_frame,pose=[0.2635,-0.0275,0.0300,0.0,0.0,0.0],dims=[0.04,0.04,0.04])
#robotArm.add_box("package$2",0.00,0.6318,0.015,0.03,0.03,0.03)
rospy.loginfo("Picking object 1")
pick_pose_1 = geometry_msgs.msg.Pose()
pick_pose_1.position.x = 0.2635
pick_pose_1.position.y = -0.0275
pick_pose_1.position.z = 0.0300
# pick_pose_1=robotArm._scene.getObject("box1").primitive_poses[0]
# rospy.loginfo(fpick_pose_1.position.x, pick_pose_1.position.y, pick_pose_1.position.z)
robotArm.pick_action(robotArm._scene, robotArm._robot, robotArm._group, robotArm._eef_group, pick_pose_1, box="box1")
# Place box1
rospy.loginfo("Placing object 1")
place_pose_1 = geometry_msgs.msg.Pose()
place_pose_1.position.x = -0.2635
place_pose_1.position.y = 0.0275
place_pose_1.position.z = 0.0300
robotArm.place_action(robotArm._scene, robotArm._group, robotArm._eef_group, place_pose_1, box="box1")
robotArm.set_pose(robotArm._group,"arm_home")
robotArm._scene.remove_world_object()
if __name__ == '__main__':
main()
Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/sync_plan.py 0 → 100755
View file @ 1985345a
#!/usr/bin/env python3
# encoding=utf-8
import time
import math
import rospy
from sensor_msgs.msg import JointState
from adafruit_servokit import ServoKit
kit = None
def callback(data):
global kit
# rospy.loginfo(rospy.get_caller_id() + "%s", data)
data_list = []
for index, value in enumerate(data.position):
radians_to_angles = round(math.degrees(value), 2)
if radians_to_angles>180:
radians_to_angles = 180
if index==1:
radians_to_angles = 90 - radians_to_angles
if index>1 and index<4 :
radians_to_angles = 90 + radians_to_angles
if index==5:
radians_to_angles = 180 + radians_to_angles
data_list.append(round(radians_to_angles,1))
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
# mc.send_angles(data_list, 25)
kit.servo[0].angle = data_list[0]
kit.servo[1].angle = data_list[1]
kit.servo[2].angle = data_list[2]
kit.servo[3].angle = data_list[3]
kit.servo[4].angle = data_list[4]
kit.servo[5].angle = data_list[5]
def listener():
rospy.init_node("motorcontrolNode", anonymous=True)
time.sleep(0.05)
rospy.Subscriber("joint_states", JointState, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == "__main__":
kit = ServoKit(channels=16)
m = 0
kit.servo[0].actuation_range = 180
kit.servo[0].set_pulse_width_range(600, 2600)
kit.servo[1].actuation_range = 180
kit.servo[1].set_pulse_width_range(600, 2600)
kit.servo[2].actuation_range = 180
kit.servo[2].set_pulse_width_range(600, 2600)
kit.servo[3].actuation_range = 180
kit.servo[3].set_pulse_width_range(600, 2600)
kit.servo[4].actuation_range = 180
kit.servo[4].set_pulse_width_range(600, 2600)
kit.servo[5].actuation_range = 180
kit.servo[5].set_pulse_width_range(600, 2600)
kit.servo[6].actuation_range = 180
kit.servo[6].set_pulse_width_range(600, 2600)
kit.servo[0].angle = 90.0
kit.servo[1].angle = 135.0
kit.servo[2].angle = 180.0
kit.servo[3].angle = 135.0
kit.servo[4].angle = 15.0
kit.servo[5].angle = 165.0
listener()
Exercises/catkin_ws/src/gut_robot/arm_robot_moveit/scripts/testmoveit.py 0 → 100755
View file @ 1985345a
#! /usr/bin/python3
import rospy
import sys
import math
import actionlib
import moveit_commander
import geometry_msgs.msg
from moveit_commander import MoveGroupCommander
from tf.transformations import quaternion_from_euler
# Initialize the move group and the robot's planning interface
move_group = MoveGroupCommander("arm_group") # Replace "arm" with your move group name
move_group.set_planning_time(12)
# # PREPICK
# target_pose = geometry_msgs.msg.Pose()
# target_pose.position.x = 0.2235# Example x position
# target_pose.position.y = -0.0233 # Example y position
# target_pose.position.z = 0.2330 # Example z position
# #angle = 45 # 45 degrees
# # angle = [-90.12,53.82,-6.046]
# #radians = angle * 3.14159 / 180.0
# # quaternion = quaternion_from_euler(angle[0]* 3.14159 / 180.0, angle[1]* 3.14159 / 180.0 ,angle[2]* 3.14159 / 180.0 )
# quaternion = quaternion_from_euler(-1.5716,0.0780, -0.1044)
# target_pose.orientation.x = quaternion[0]
# target_pose.orientation.y = quaternion[1]
# target_pose.orientation.z = quaternion[2]
# target_pose.orientation.w = quaternion[3]
# # PICK
# target_pose = geometry_msgs.msg.Pose()
# target_pose.position.x = 0.2635 # Example x position
# target_pose.position.y = -0.0275 # Example y position
# target_pose.position.z = 0.0300 # Example z position
# #radians = angle * 3.14159 / 180.0
# quaternion = quaternion_from_euler(-1.5716,0.7727, -0.1050)
# target_pose.orientation.x = quaternion[0]
# target_pose.orientation.y = quaternion[1]
# target_pose.orientation.z = quaternion[2]
# target_pose.orientation.w = quaternion[3]
# #INTERMEDIATE
# target_pose = geometry_msgs.msg.Pose()
# target_pose.position.x = 0.0248 # Example x position
# target_pose.position.y = 0.2273 # Example y position
# target_pose.position.z = 0.2330 # Example z position
# #angle = 45 # 45 degrees
# #radians = angle * 3.14159 / 180.0
# quaternion = quaternion_from_euler(-1.5710, -0.0002, 1.4621)
# target_pose.orientation.x = quaternion[0]
# target_pose.orientation.y = quaternion[1]
# target_pose.orientation.z = quaternion[2]
# target_pose.orientation.w = quaternion[3]
# PRE PLACE
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = -0.2256 # Example x position
target_pose.position.y = 0.0275 # Example y position
target_pose.position.z = 0.2310 # Example z position
#angle = 45 # 45 degrees
#radians = angle * 3.14159 / 180.0
quaternion = quaternion_from_euler(-1.5434, 0.0167, 3.0377)
target_pose.orientation.x = quaternion[0]
target_pose.orientation.y = quaternion[1]
target_pose.orientation.z = quaternion[2]
target_pose.orientation.w = quaternion[3]
# #PLACE
# target_pose = geometry_msgs.msg.Pose()
# target_pose.position.x = -0.2635 # Example x position
# target_pose.position.y = 0.0275 # Example y position
# target_pose.position.z = 0.0300 # Example z position
# #radians = angle * 3.14159 / 180.0
# quaternion = quaternion_from_euler(-1.5716,0.7727,-0.1050)
# target_pose.orientation.x = quaternion[0]
# target_pose.orientation.y = quaternion[1]
# target_pose.orientation.z = quaternion[2]
# target_pose.orientation.w = quaternion[3]
# Set the target pose
move_group.set_pose_target(target_pose)
# Plan and execute the motion
plan = move_group.go(wait=True)
Exercises/catkin_ws/src/pick_and_place/scripts/pick_place_example.py 100644 → 100755
View file @ 1985345a
......@@ -12,6 +12,7 @@ from geometry_msgs.msg import Pose
from shape_msgs.msg import SolidPrimitive
from moveit_msgs.msg import *
import tf
import actionlib
def create_object(obj_id, ref_frame, dimensions, pose):
'''
......@@ -62,6 +63,7 @@ def create_object(obj_id, ref_frame, dimensions, pose):
#Adding Object
object.primitive_poses = [object_pose]
object.operation = object.ADD
print(f"Object {obj_id} created with pose: {object_pose.position.x}, {object_pose.position.y}, {object_pose.position.z}")
return object
def add_items(scene, frame):
......@@ -76,20 +78,20 @@ def add_items(scene, frame):
Returns the items that can be picked as an array.
'''
rospy.loginfo("-- Adding Objects --")
height_from_ground = 0.3
height_from_ground = 0.0 #change this to move the table up or down
#Define the objects
table1 = create_object(obj_id="table1", ref_frame=frame,
dimensions=[0.2, 0.5, 0.01],
pose=[0.5, 0, height_from_ground, 0.0, 0.0, 0.0])
dimensions=[0.1, 0.5, 0.01],
pose=[0.2, 0.0, height_from_ground, 0.0, 0.0, 0.0],)
table2 = create_object(obj_id="table2", ref_frame=frame,
dimensions=[0.2, 0.5, 0.01],
pose=[-0.5, 0, height_from_ground, 0.0, 0.0, 0.0])
dimensions=[0.1, 0.5, 0.01],
pose=[-0.2, 0.0, height_from_ground, 0.0, 0.0, 0.0])
box1 = create_object(obj_id="box1", ref_frame=frame,
dimensions=[0.1, 0.025, 0.025],
pose=[0.5, 0, height_from_ground+(0.05/2.0), 0.0, 0.0, 0.0])
dimensions=[0.05, 0.025, 0.025],
pose=[0.2, 0, height_from_ground+(0.05/2.0), 0.0, 0.0, 0.0])
#Add Objects to the scene
scene.add_object(table1)
......@@ -97,6 +99,28 @@ def add_items(scene, frame):
scene.add_object(box1)
rospy.sleep(2) #Waiting to spawn the objects in the world
def set_gripper_position(gripper, gripper_action):
'''
Function to set named position to open and close gripper
Args:
gripper: group_name of gripper in the manipulator
gripper_action: action to perform. Allowed values ["gripper_open", "gripper_close"]
Returns:
None.
'''
execute_trajectory_client = actionlib.SimpleActionClient('execute_trajectory', ExecuteTrajectoryAction)
execute_trajectory_client.wait_for_server()
gripper.set_named_target(gripper_action)
plan_success, plan, planning_time, error_code = gripper.plan()
goal=ExecuteTrajectoryGoal()
goal.trajectory = plan
execute_trajectory_client.send_goal(goal)
execute_trajectory_client.wait_for_result()
rospy.loginfo('\033[32m' + "Now at: {}".format(gripper_action) + '\033[0m')
def gripper_action(gripper_end, action="open"):
'''
Function to execute the required action of the gripper.
......@@ -112,13 +136,13 @@ def gripper_action(gripper_end, action="open"):
print("Executing gripper action = ", action)
if(action == "open"):
gripper_end.move(gripper_end.max_bound(), True)
set_gripper_position(gripper_end,"gripper_open")
elif(action == "close"):
gripper_end.move(gripper_end.max_bound()*0.5, True)
set_gripper_position(gripper_end,"gripper_close")
else:
print("Action undefined")
def move_to_pose(arm, goal_pose):
def move_to_pose(arm, goal_pose, goal_angle):
'''
Function to move the robot arm to the requested pose
......@@ -131,18 +155,15 @@ def move_to_pose(arm, goal_pose):
None
'''
#NOTE:
#The planner will create a plan for the panda_link8 to reach
#the goal pose. But since we have an end-endeffector attached to it
#it will cause collision.
#distance between wrist(panda_link8) and gripper_end(panda_hand_tcp)
wrist_to_tcp = 0.103
#The planner will create a plan for the gripper_center to reach
#the goal pose.
# rospy.logwarn("planning and moving to location")
gripper_angle = geometry_msgs.msg.Quaternion()
#Orientation of the tcp(panda_hand_tcp) w.r.t base frame(panda_link0)
quaternion = tf.transformations.quaternion_from_euler(math.radians(180), 0, math.radians(-45))
#Orientation of the tcp(gripper_center) w.r.t base frame(world)
rpy = [math.radians(goal_angle[0]), math.radians(goal_angle[1]), math.radians(goal_angle[2])]
quaternion = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
gripper_angle.x = quaternion[0]
gripper_angle.y = quaternion[1]
......@@ -151,16 +172,16 @@ def move_to_pose(arm, goal_pose):
#defining pre-grasp position
grasp_pose = Pose()
grasp_pose.position.x = goal_pose.position.x
grasp_pose.position.y = goal_pose.position.y
#adding tcp distance because the planning frame is panda_link8
grasp_pose.position.z = goal_pose.position.z + wrist_to_tcp
grasp_pose.position.x = goal_pose[0] #x position of the goal pose
grasp_pose.position.y = goal_pose[1] #y position of the goal pose
grasp_pose.position.z = goal_pose[2]
grasp_pose.orientation = gripper_angle
print("Exectuting move_to_pose ({} , {}, {})"
.format(grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z))
print("Exectuting move_to_pose ({} , {}, {}), ({}, {}, {})"
.format(grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z,
goal_angle[0], goal_angle[1], goal_angle[2]))
arm.set_goal_position_tolerance(0.001) #setting tolerence for pose
arm.set_goal_position_tolerance(0.01) #setting tolerence for pose
arm.set_pose_target(grasp_pose) #setting the pose of the end-effector
plan = arm.go(wait=True) #move the arm to the grasp_pose.
......@@ -188,11 +209,11 @@ def initialize_robot_params():
rospy.loginfo("--- MoveIt APIs initialising ---")
scene = moveit_commander.PlanningSceneInterface()
robot_commander = moveit_commander.RobotCommander()
gripper = robot_commander.get_joint('panda_finger_joint1')
gripper = moveit_commander.MoveGroupCommander("gripper_group")
rospy.sleep(3) #essential to intialize the API's
group_name = "panda_arm" #group name of the robot arm we are using
group_name = "arm_group" #group name of the robot arm we are using
move_group = moveit_commander.MoveGroupCommander(group_name)
move_group.set_goal_position_tolerance(1E-2)
......@@ -247,16 +268,17 @@ def main():
rospy.loginfo("------------------")
rospy.loginfo("Moving to pose1")
pose1 = Pose()
pose1.position.x = 0.5 #x positin in base frame
pose1.position.y = 0.0
pose1.position.z = 0.5
move_to_pose(arm, pose1) #sending goal pose
position1 = [0.231, -0.022, 0.103]
orientation1 = [-90.174, 41.199, -5.554]
move_to_pose(arm, position1, orientation1) #sending goal pose
rospy.loginfo("Moving to pose2")
pose2 = copy.deepcopy(pose1) #making a copy of the pose
pose2.position.x += -1 #creating a different x value
move_to_pose(arm, pose2) #sending the goal pose
position2 = position1 #copying the position from pose1
orientation2 = [-89.870, 41.196, 173.669] #copying the orientation from pose1
position2[0] = -1*position2[0] #creating a different x value
position2[1] = -1*position2[1] #creating a different y value
move_to_pose(arm, position2, orientation2) #sending the goal pose
if __name__ == '__main__':
#starting point of the code
......
Exercises/python-exercises/3-Forward-Kinematics-DH-Table.ipynb
View file @ 1985345a
%% Cell type:markdown id:e4ccc978 tags:
# Forward Kinematics - Denavit-Hartenberg Matrix
## Task Objective
The aim of this task will be to use your understanding of the `sympy` package from the previous exercise and use the concept of **DH Table** to compute tranformation matrices that will give you the pose information of each link. You can get a **total of 20 points** if you complete the entire task successfully. You will receive partial points as per your implementation. The points for each task is mentioned in the comments in the format `(Points: *)`
%% Cell type:markdown id:79d129c2 tags:
### Exercise 1:
![task1](task_questions/task1.png)
![task description](task_questions/task-3.png)
%% Cell type:markdown id:d138648a tags:
The frame attachment information and the DH table of the Puma 560 robot are given below.
The **constant parameters** are
- $h$ = 40 cm,
- $a_2$ = 50 cm,
- $a_3$ = 10 cm,
- $d_3$ = 20 cm and
- $d_4$ = 30 cm.
1. Find transformation matrices between every consectuive frame.
**Remember**:
${}_i^{i-1}T =
\begin{bmatrix}
c_{\theta_i} & -s_{\theta_i} & 0 & a_i \\
s_{\theta_i}c_{\alpha_i} & c_{\theta_i}c_{\alpha_i} & -s_{\alpha_i} & -s_{\alpha_i} d_i\\
s_{\theta_i}s_{\alpha_i} & c_{\theta_i}s_{\alpha_i} & c_{\alpha_i} & c_{\alpha_i} d_i\\
0 & 0 & 0 & 1\\
\end{bmatrix}$
where $c_*, s_*$ represents `cos` and `sin` respectively.
%% Cell type:code id:856e8c26 tags:
``` python
# importing necessary modules
import math
import sympy as sym
#setting the precision for decimal values.
#setting the precision for decimal values.
%precision 4
sym.init_printing() #this will make all the symbolic print pretty.
```
%% Cell type:markdown id:3e160bf2 tags:
Define your constant parameter **$h$**, **$a_i$** and **$d_i$**. You can define the variable parameters **$\theta_i$** using the [`Symbol`](https://www.tutorialspoint.com/sympy/sympy_symbols.htm) function available in the `sympy` library. **A quick start tutorial to the sympy library can be found [here](https://docs.sympy.org/latest/tutorials/intro-tutorial/gotchas.html).** We use the library [sympy](https://www.sympy.org/en/index.html) in the context of this task to allow for matrix multiplication with elements of the matrix being symbolic, since **the $\theta_i$ values are variable parameters.**
%% Cell type:code id:2a7a4011 tags:
``` python
#YOUR CODE HERE
#Define the constant parameters (Points: 2)
#Define symbols for all theta
#Example for theta_1 is shown below. You need to define other theta variables (Points: 2)
theta_1 = sym.Symbol('theta_1')
```
%% Cell type:markdown id:023cc991 tags:
Create a table to store the value of the DH table.
**Hint:** Try using the [array](https://docs.sympy.org/latest/modules/tensor/array.html#module-sympy.tensor.array) function from the `sympy` library to create an N-dimensional array. Each row of the array will represent row from the DH table.
%% Cell type:code id:6fb13c2f tags:
``` python
#YOUR CODE HERE
#Fill in the array variable to match the table given in the question. (Points: 2)
#each row of the dh_table corresponds to each row from the DH table shown in the question
dh_table = sym.Array([])
```
%% Cell type:code id:13a74e36-b3e5-49d2-a741-a47e633fde6f tags:
``` python
#EVALUATION CELL
#This cell is used to evaluate your implementation using some fixed values.
#If this cell prints nothing then it means your implementation is correct.
#Otherwise this cell will print some error.
assert dh_table.shape == (6, 4), "Shape is incorrect"
```
%% Cell type:markdown id:af6b38c9 tags:
Create a function `calc_transformation` that takes as input the values from each row of the DH table and then calculates the transformation matrix ${}_i^{i-1}T$ for each row of the DH Table.
**Hint:** Use the [Matrix](https://docs.sympy.org/latest/tutorials/intro-tutorial/matrices.html) and [trigonometric](https://docs.sympy.org/latest/modules/functions/elementary.html#trigonometric-functions) functions provided in the `sympy` package
%% Cell type:code id:5ae5f46a tags:
``` python
'''
Function to calculate the transformation matrix between frames `i` and `i-1`
Inputs:
alpha, a, d, theta: values from the DH table
Return:
T: the transformation matrix between `i` and `i-1`
'''
def calc_transformation(alpha, a, d, theta):
#Create the matrix (Points: 2)
T = sym.Matrix()
return T
```
%% Cell type:code id:fd3975c1 tags:
``` python
n_frame = dh_table.shape[0] #shape() function gives a tuple with values row, columns
T_list = [] #array to store the transformation matrices
for i in range(0,n_frame):
T = calc_transformation(alpha=dh_table[i,0], a=dh_table[i,1], d=dh_table[i,2], theta=dh_table[i,3])
T_list.append(T)
```
%% Cell type:markdown id:56230bbd tags:
Print the transformation matrix ${}_1^0T$
**Note:** Do not use the python's `print` function while printing the matrix. This will make sure you have a pretty rendering of the matrix. You can try using the `print` function to see the difference.
If your implementation was correct, then **your result for the transformation ${}_1^0T$ will look like following matrix**.
${}_1^0T =
\begin{bmatrix}
cos(\theta_1) & -sin(\theta_1) & 0 & 0 \\
sin(\theta_1) & cos(\theta_1) & 0 & 0\\
0 & 0 & 1 & 0.4\\
0 & 0 & 0 & 1\\
\end{bmatrix}$
%% Cell type:code id:fb9fff68 tags:
``` python
#Print statement here
#Note that we are not using the `print` function to print the value
#Print transformation matrix (Points: 2)
T_list[0]
```
%% Cell type:markdown id:2fbdb3d4 tags:
Print the transformation matrix ${}_2^1T$:
%% Cell type:code id:aee3978b tags:
``` python
#Print statement here
```
%% Cell type:markdown id:08d7ff99 tags:
Print the transformation matrix ${}_3^2T$:
%% Cell type:code id:4e2ceb82 tags:
``` python
#Print statement here
```
%% Cell type:markdown id:66b1f49b tags:
Print the transformation matrix ${}_4^3T$:
%% Cell type:code id:15a390dc tags:
``` python
#Print statement here
```
%% Cell type:markdown id:5f1a09eb tags:
Print the transformation matrix ${}_5^4T$:
%% Cell type:code id:a2bda339 tags:
``` python
#Print statement here
```
%% Cell type:markdown id:139344a9 tags:
Print the transformation matrix ${}_6^5T$:
%% Cell type:code id:71646159 tags:
``` python
#Print statement here
```
%% Cell type:markdown id:9ec76206-53f3-49c9-b6a5-dd0b5019f5db tags:
## Calculate the angular jacobian matrix.
2. Find the angular Jacobian matrix ($J_{\omega}$) between the world frame and the end-effector.
**Remember**:
$J_{\omega} = \begin{bmatrix}
{}_1^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_2^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_3^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_4^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_5^0R \begin{bmatrix}0\\0\\1\end{bmatrix} & {}_6^0R \begin{bmatrix}0\\0\\1\end{bmatrix}
\end{bmatrix}$
Steps to follow:
1. Get the rotation matrix from the transformation matrix.
2. Do a matrix-vector multiplication between the **rotation matrix** and the vector
$\begin{bmatrix} 0 \\ 0\\ 1 \end{bmatrix}$ . This matrix-vector multiplication will produce a `3x1` vector which we will concatenate together to form the final jacobian $J_{\omega}$
**Some useful tips:**
If you define a single row in the `Matrix` function of the `sympy` package, then it will be taken as a vector. **Do not get confused between using the same `Matrix` function to create both a matrix and a vector**. The number of rows you define will decide if you are creating a matrix of a vector.
- 1 row -> Vector
- More than 1 row -> Matrix
You can use the `@` operator available in the `sympy` package to do `Matrix-vector` multiplication.
%% Cell type:code id:242bc822-ad2c-4cfe-9f6a-d67489a233dc tags:
``` python
#example of how to define a vector.
vec_1 = sym.Matrix([1,2,3])
#printing the vector. Note that you will get a pretty looking vector form if you don't use the print() function.
#uncomment the following line to print the vector
#vec_1
#matric vector multiplication.
mat_ = sym.Matrix([[1,2,3],
[4,5,6],
[7,8,9]]) #pay attention to more rows we have added here
result = mat_@vec_1 # <--- matrix vector multiplication
result #printing the result of matrix-vec multiplication
```
%% Cell type:code id:a10e031d-cfeb-4839-b6e0-2a0b7d906675 tags:
``` python
#You can compute the cross product of two vectors using the `cross` function as shown below.
vec_2 = sym.Matrix([4,5,6])
cross_prod = vec_1.cross(vec_2)
cross_prod #printing the cross product
```
%% Cell type:markdown id:24239c3e-f2a7-40f7-92b7-421b5c54e0e4 tags:
**Step 1:**
Create a function `get_rotation_matrix` that takes as input the transformation matrix and return the `3x3` rotation matrix from the transformation matrix.
***Hint:*** You can use [matrix slicing](https://docs.sympy.org/latest/modules/matrices/matrices.html#basic-manipulation:~:text=also%20able%20to-,slice%20submatrices,-%3B%20slices%20always%20give) to easily obtain the rotation matrix.
%% Cell type:code id:b0d16468-94e2-4a11-b7ed-6092a004a679 tags:
``` python
def get_rotation_matrix(tf_matrix):
#YOUR CODE HERE (points: 2)
#rot_matrix=
return rot_matrix
```
%% Cell type:markdown id:ee1a342f-940c-403b-9b92-e5719b54c060 tags:
**Step 2:**
- Create a loop that will recurcively get the rotation matrix from the transformation matrix.
- Then compute the matrix-vector multiplication.
- Finally push the vector to the corresponding column of the jacobian matrix.
%% Cell type:code id:bcd202f7-9fd5-4834-937e-81b28d6e019b tags:
``` python
angular_jacobian = sym.zeros(3,6) #creating a zero matrix to update the final value
for i in range(0, len(T_list)):
#1.compute the rotation matrix using the `get_rotation_matrix` function. (point: 2)
#rotation_matrix =
#2.create the vector [0,0,1] (point: 2)
#unit_vec =
#3.compute the matrix-vector multiplication (point: 2)
#result_ =
#4.push the resulting vector to the corresponding colum of the jacobian matrix. (point: 2)
angular_jacobian[:,i] = result_
#printing the final angular_jacobian
angular_jacobian
```
......
Exercises/python-exercises/task_questions/task-3.png 0 → 100644
View file @ 1985345a
Exercises/python-exercises/task_questions/task-3.png

128 KiB

Exercises/python-exercises/task_questions/task1.png deleted 100644 → 0
View file @ 863085fa
Exercises/python-exercises/task_questions/task1.png

496 KiB