def main():
rospy.init_node('cartesian_move_test', anonymous=True)
raw_data = np.load('record_demo.npy')
# raw_data = np.load('./demonstrations/record_demo_1.npy')
filtered_data = gaussian_filter1d(raw_data.T, sigma=5).T
filtered_data = util.filter_static_points(filtered_data, 0.03)
# util.plot_3d_demo(filtered_data)
# moveit
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander('manipulator')
# reference_frame = 'iiwa_link_ee'
# group.set_pose_reference_frame(reference_frame)
# joint_command = [0.11133063584566116, 0.5712737441062927,
# -0.09774867445230484, -1.7133415937423706, -0.036780450493097305, -0.5433750152587891, 0.17699939012527466]
util.speed_set(group, 0.03)
# plan = util.fK_point_calculate(group,JointAngle=joint_command)
# util.execute_plan(group,plan)
for point in filtered_data:
plan = util.iK_point_calculate(group, point=point)
util.execute_plan(group, plan)
# # ipdb.set_trace()
# plan = util.ik_cartesain_path(group, filtered_data[:,:])
# #
# # plan = util.set_trajectory_speed(plan, 0.2)
# # ipdb.set_trace()
util.execute_plan(group, plan)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def main():
rospy.init_node('move_test', anonymous=True)
# moveit
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander('manipulator')
plan = util.fK_point_calculate(group, JointAngle=joint_1)
util.execute_plan(group, plan)
plan = util.fK_point_calculate(group, JointAngle=joint_2)
util.execute_plan(group, plan)
plan = util.fK_point_calculate(group, JointAngle=joint_3)
util.execute_plan(group, plan)
plan = util.fK_point_calculate(group, JointAngle=joint_1)
util.execute_plan(group, plan)
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
#!/usr/bin/env python
import numpy as np
import pandas as pd
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point, Pose
import ipdb
from scipy.ndimage.filters import gaussian_filter1d
from birl_pydmps import util
joint_command = [0, 0, 0, 0, 0, 0, 0]
rospy.init_node('move_test_1', anonymous=True)
# moveit
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander('manipulator')
util.speed_set(group, 0.01)
plan = util.fK_point_calculate(group, JointAngle=joint_command)
util.execute_plan(group, plan)
# ipdb.set_trace()
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def main():
rospy.init_node('cartesian_move_test', anonymous=True)
#set twintool for gripper init
print('initial twintool joint')
twintool_change_ee(True)
gripper(False)
# return
# moveit ros
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander('arm')
scene = moveit_commander.PlanningSceneInterface()
planning_frame = group.get_planning_frame()
print "============ Reference frame: %s" % planning_frame
eef_link = group.get_end_effector_link()
print "============ End effector: %s" % eef_link
group_names = robot.get_group_names()
print "============ Robot Groups:", robot.get_group_names()
print "============ Printing robot state"
print robot.get_current_state()
print ""
# init pose
joint_command = [0, 0, 0, 0, 0, -pi / 2, 0]
# util.speed_set(group,0.5)
plan = util.fK_point_calculate(group, JointAngle=joint_command)
util.execute_plan(group, plan)
rospy.sleep(2.0)
marker1_topic = "/object/box"
marker2_topic = "/object/karaage"
marker3_topic = "/object/onigiri"
marker4_topic = "/object/chip"
print("wait makers message")
marker1_msg = rospy.wait_for_message(marker1_topic, Marker, timeout=3)
marker2_msg = rospy.wait_for_message(marker2_topic, Marker, timeout=3)
# point transform to world
print("wait for listener")
listener = tf.TransformListener()
listener.waitForTransform("/world", "/camera_color_optical_frame",
rospy.Time(0), rospy.Duration(4.0))
# ipdb.set_trace()
transformed_point = []
transformed_point.append(copy.deepcopy(PointStamped()))
transformed_point[0].header = marker1_msg.header
# ipdb.set_trace()
transformed_point[0].point = marker1_msg.pose.position
transformed_point[0].header.stamp = rospy.Time(0)
transformed_point[0] = listener.transformPoint("/world",
transformed_point[0])
transformed_point[0].point.z += 0.02
transformed_point.append(copy.deepcopy(PointStamped()))
transformed_point[1].header = marker2_msg.header
transformed_point[1].point = marker2_msg.pose.position
transformed_point[1].header.stamp = rospy.Time(0)
transformed_point[1] = listener.transformPoint("/world",
transformed_point[1])
transformed_point[1].point.x -= 0.03
transformed_point[1].point.y += 0.015
transformed_point[1].point.z -= 0.04
print(transformed_point)
print('len marker', len(transformed_point))
for i in range(len(transformed_point)):
print('i ', i)
waypoints = []
#prepick
wpose = Pose()
wpose.position.x = transformed_point[1].point.x
wpose.position.y = transformed_point[1].point.y
wpose.position.z = transformed_point[1].point.z + 0.1
wpose.orientation.x = 2.6944e-5
wpose.orientation.y = 0.70713
wpose.orientation.z = 2.5173e-5
wpose.orientation.w = 0.70708
waypoints.append(copy.deepcopy(wpose))
#pick
wpose.position.z = transformed_point[1].point.z
wpose.orientation.x = 2.6944e-5
wpose.orientation.y = 0.70713
wpose.orientation.z = 2.5173e-5
wpose.orientation.w = 0.70708
waypoints.append(copy.deepcopy(wpose))
# group.set_max_velocity_scaling_factor(0.01)
for i in range(6):
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
group.execute(plan, wait=True)
gripper(True)
rospy.sleep(1.5)
waypoints = []
wpose.position.z = wpose.position.z + 0.1
waypoints.append(copy.deepcopy(wpose))
wpose.position.x = transformed_point[0].point.x
wpose.position.y = transformed_point[0].point.y
wpose.position.z = transformed_point[0].point.z + 0.1
wpose.orientation.x = 2.6944e-5
wpose.orientation.y = 0.70713
wpose.orientation.z = 2.5173e-5
wpose.orientation.w = 0.70708
waypoints.append(copy.deepcopy(wpose))
# place
wpose.position.x = transformed_point[0].point.x
wpose.position.y = transformed_point[0].point.y
wpose.position.z = transformed_point[0].point.z
wpose.orientation.x = 2.6944e-5
wpose.orientation.y = 0.70713
wpose.orientation.z = 2.5173e-5
wpose.orientation.w = 0.70708
waypoints.append(copy.deepcopy(wpose))
group.set_max_velocity_scaling_factor(0.01)
for i in range(6):
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
group.execute(plan, wait=True)
gripper(False)
rospy.sleep(1.5)
waypoints = []
wpose.position.x = transformed_point[0].point.x
wpose.position.y = transformed_point[0].point.y
wpose.position.z = transformed_point[0].point.z + 0.1
wpose.orientation.x = 2.6944e-5
wpose.orientation.y = 0.70713
wpose.orientation.z = 2.5173e-5
wpose.orientation.w = 0.70708
waypoints.append(copy.deepcopy(wpose))
group.set_max_velocity_scaling_factor(0.01)
for i in range(5):
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
group.execute(plan, wait=True)
break
# init pose
joint_command = [0, 0, 0, 0, 0, -pi / 2, 0.0]
util.speed_set(group, 1)
plan = util.fK_point_calculate(group, JointAngle=joint_command)
util.execute_plan(group, plan)
group.stop()
group.clear_pose_targets()
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
