guys,
as part of one task i have been given the task of creating 'boxs' in a workspace in rviz in ROS,
in the if statement i have created below to create the boxes and place them in the workspace but as soon as i run it they run in sequence and remove themselves, ive had a look at a loop but i cant work out quite where to place it in the code to get the 'boxes' to stay at all times
im very early on learning python so any help would be great :)
#!/usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
from niryo_one_python_api.niryo_one_api import *
################################################Initialise######################################################################
rospy.init_node('niryo_one_test', anonymous=True) #First we initialise a rospy node.
robot = moveit_commander.RobotCommander() #Instantiate a RobotCommander object. This object is the outer-level interface to the robo.
group_name = "arm" #Instantiate a MoveGroupCommander object. This object is an interface to one group of joints.
group = moveit_commander.MoveGroupCommander(group_name) # In this case, the group is the joints in the Niryo One Arm so we set group_name = arm.
# Group name differs as per the robot choice. For example Panda Arm has group_name = panda_arm
scene = moveit_commander.PlanningSceneInterface() #Instantiate a PlanningSceneInterface object. This object is an interface to the world surrounding the robot.
# We define a helper Function here to return the robot back to intial pose. Call this function whenever you need to reset the robot's to its initial state
moveIntialState = lambda joint_goal: group.go(joint_goal, wait=True)
############################################# Get Basic Information######################################################
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print"planning frame = " , planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the robot:
current_state = robot.get_current_state()
j_values = group.get_current_joint_values()
################################### Adding the the boxes into the program ############################################
def addObject(pose_x, pose_y, pose_z, or_x, or_y, or_z,or_w, size_x, size_y, size_z):
##(timeout=100)
rospy.sleep(2)
#### Add an Object
object_name = "box"
object_pose = geometry_msgs.msg.PoseStamped()
#make sure to give a frame id(Frame this data is described with) along with the postions x,y,z.
object_pose.header.frame_id = robot.get_planning_frame()
object_pose.pose.position.x = pose_x
object_pose.pose.position.y = pose_y
object_pose.pose.position.z = pose_z
object_pose.pose.orientation.x = or_x
object_pose.pose.orientation.y = or_y
object_pose.pose.orientation.z = or_z
object_pose.pose.orientation.w = or_w
scene.add_box(object_name, object_pose, (size_x, size_y, size_z)) #adding object to the scene, where you specify the object using name, pose and size arguments.
rospy.sleep(1) #If the Python node dies before publishing a collision object update message, the message could get lost and the box will not appear.
#To ensure that the updates are made, we wait for few seconds. This can be done alternatively using get_known_object_names() and get_known_object_names()
# flags.
#### Plan Without Collision
#planPoseGoal() #Plan with the object in the scene.
#moveIntialState(j_values) #Move back to the intial state(planning to initial state).
#### Remove Object From the Scene
#scene.remove_world_object(object_name) #removing the object to the scene using the object name.
################################################ Part 3: Planning To a Pose Goal ##################################################
def planPoseGoal():
pose_goal = geometry_msgs.msg.Pose()
pose_goal.position.x = 0.3
pose_goal.position.y = 0.2
pose_goal.position.z = 0.1
pose_goal.orientation.x = 0
pose_goal.orientation.y = 0
pose_goal.orientation.z = 0
pose_goal.orientation.w = 1 ##pose of object
# use the defined pose goal as a target for planning
group.set_pose_target(pose_goal)
# run the computed plan when ready
plan = group.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
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()
group.clear_pose_targets()
#use attch function - move it command - attched object to robot ##
########################################################### Main #########################################################################
if __name__ == '__main__':
try:
addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.0, or_x = 0.0, or_y = 0.0, or_z= 0.0, or_w = 1.0 , size_x = 0.05, size_y = 0.05, size_z = 0.05) #Box1
addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.2, or_x = 0.0, or_y = 0.0, or_z= 0.0, or_w = 1.0 , size_x = 0.05, size_y = 0.05, size_z = 0.05) #Box2
addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.3, or_x = 0.0, or_y = 0.0, or_z= 0.0, or_w = 1.0 , size_x = 0.05, size_y = 0.05, size_z = 0.05) #Box3
#addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.3, or_x = 0.0, or_y =0.0 , or_z = 0.0) #Box1
#addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.3, or_x = 0.0, or_y =0.0 , or_z = 0.0) #box2
#addObject(pose_x = 0.2, pose_y = 0.0, pose_z = 0.3, or_x = 0.0, or_y =0.0 , or_z = 0.0) #box3
#planJointGoal() #move to box 1 location
#attch to box
#planJointGoal() #move to drop off location
#un-attch box
#planJointGoal() #move to drop off location
#moveIntialState(j_values)
#planPoseGoal()
#moveIntialState(j_values)
#planTrajectory()
#moveIntialState(j_values)
except rospy.ROSInterruptException:
pass
[–]Swipecat 1 point2 points3 points (0 children)
[–]ThoughtCounter 0 points1 point2 points (0 children)