Sawyer cartesian control

test_cartesian.py

#!/usr/bin/env python
import rospy
import numpy as np
import moveit_commander
import intera_interface
from copy import deepcopy
from geometry_msgs.msg import Pose
import sys
from visual_mpc.envs.robot_envs.sawyer.sawyer_impedance import SawyerImpedanceController
from visualization_msgs.msg import Marker
import tf

pub = rospy.Publisher('visualization_marker', Marker, queue_size=10)

ctrlr = SawyerImpedanceController(gripper_attached='sawyer_gripper')
ctrlr.move_to_neutral()
rospy.sleep(1)

# robot = moveit_commander.RobotCommander()
# group = moveit_commander.MoveGroupCommander('right_arm')
curr_pose = ctrlr.get_xyz_quat()

pose = Pose()
pose.position.x = curr_pose[0][0]
pose.position.y = curr_pose[0][1]
pose.position.z = curr_pose[0][2]
pose.orientation.w = curr_pose[1][0]
pose.orientation.x = curr_pose[1][1]
pose.orientation.y = curr_pose[1][2]
pose.orientation.z = curr_pose[1][3]

curr_marker = Marker()
curr_marker.header.frame_id = "/world"
curr_marker.header.stamp = rospy.get_rostime()
curr_marker.ns = ""
curr_marker.id = 0
curr_marker.type = 0 # arrow
curr_marker.action = 0 # add/modify
curr_marker.pose = pose
curr_marker.scale.x = 0.1
curr_marker.scale.y = 0.02
curr_marker.scale.z = 0.02
curr_marker.color.r = 1
curr_marker.color.g = 0
curr_marker.color.b = 0
curr_marker.color.a = 1
curr_marker.lifetime = rospy.Duration(0) # no timeout
pub.publish(curr_marker)

new_pose = deepcopy(curr_pose)
new_pose[0][1] += 0.1
new_pose[0][2] += 0.2

pose1 = Pose()
pose1.position.x = new_pose[0][0]
pose1.position.y = new_pose[0][1]
pose1.position.z = new_pose[0][2]
pose1.orientation.w = new_pose[1][0]
pose1.orientation.x = new_pose[1][1]
pose1.orientation.y = new_pose[1][2]
pose1.orientation.z = new_pose[1][3]

new_marker = deepcopy(curr_marker)
new_marker.pose = pose1
new_marker.id = 1
new_marker.color.r = 0
new_marker.color.g = 1
pub.publish(new_marker)

pose = np.array([new_pose[0][0], new_pose[0][1], new_pose[0][2], new_pose[1][0], new_pose[1][1], new_pose[1][2], new_pose[1][3]])

ctrlr.move_to_eep(pose)

# rospy.init_node('grasp_actionserver')
# robot = moveit_commander.RobotCommander()
# group = moveit_commander.MoveGroupCommander('right_arm')
# group.set_max_velocity_scaling_factor(0.2)

# curr_pose = group.get_current_pose().pose
# new_pose = deepcopy(curr_pose)
# new_pose.position.z += 0.2

# # waypoints = [curr_pose, new_pose]
# # (plan, fraction) = group.compute_cartesian_path(
#                             #    waypoints,   # waypoints to follow
#                             #    0.005,       # eef_step
#                             #    0.0)         # jump_threshold
# # if not plan:
#     # sys.exit(1)

# # group.execute(plan, wait=True)]
# group.set_pose_target(new_pose)
# group.go(wait=True)
# group.stop()
# rospy.loginfo('Moving to pose with cartesian complete.')