if __name__ == "__main__":
# global goal_pos, goal_ori, ctrl_thread
rospy.init_node("ts_control_sim_only")
# when using franka_ros_interface, the robot can be controlled through and
# the robot state is directly accessible with the API
# If using the panda_robot API, this will be
# panda = PandaArm()
robot = ArmInterface()
# when using the panda_robot interface, the next 2 lines can be simplified
# to: `start_pos, start_ori = panda.ee_pose()`
ee_pose = robot.endpoint_pose()
start_pos, start_ori = ee_pose['position'], ee_pose['orientation']
goal_pos, goal_ori = start_pos, start_ori
# start controller thread
rospy.on_shutdown(_on_shutdown)
rate = rospy.Rate(publish_rate)
ctrl_thread = threading.Thread(target=control_thread, args = [rate])
ctrl_thread.start()
# ------------------------------------------------------------------------------------
server = InteractiveMarkerServer("basic_control")
position = Point( start_pos[0], start_pos[1], start_pos[2])
marker = destination_marker.makeMarker( False, InteractiveMarkerControl.MOVE_ROTATE_3D, \
if __name__ == '__main__':
rospy.init_node("path_testing")
realarm = ArmInterface()
q0 = realarm.convertToList(realarm.joint_angles())
startq = realarm.joint_angles()
while True:
# Rather than peturbing pose, perturb q as a hack
delta_q = numpy.random.rand(7) * 0.2
q1_seed = numpy.add(q0, delta_q)
raw_input("seed")
realarm.move_to_joint_positions(realarm.convertToDict(q1_seed))
p1 = realarm.endpoint_pose()
raw_input("q0")
realarm.move_to_joint_positions(realarm.convertToDict(q0))
p0 = realarm.endpoint_pose()
raw_input("p1")
realarm.set_cart_impedance_pose(p1)
raw_input("record?")
p2 = realarm.endpoint_pose()
q1 = realarm.joint_angles()
all_results = [q0, realarm.convertToList(q1), p0, p1, p2]
raw_input("next?")
break
with open('t28.npy', 'wb') as f:
numpy.save(f, all_results)
@info:
commands robot to move to neutral pose
"""
import rospy
import copy
import IPython
import numpy
from franka_interface import ArmInterface
if __name__ == '__main__':
rospy.init_node("path_testing")
arm = ArmInterface()
frames = arm.get_frames_interface()
start_pose = arm.endpoint_pose()
pose0 = copy.deepcopy(start_pose)
pose1 = copy.deepcopy(pose0)
pose1['position'][0] += 0.1
pose2 = copy.deepcopy(pose1)
pose2['position'][1] += 0.1
pose3 = copy.deepcopy(pose2)
pose3['position'][0] -= 0.1
pose4 = copy.deepcopy(pose3)
pose4['position'][1] -= 0.1
path = [pose1, pose2, pose3, pose4]
IPython.embed()
