plt.plot(sensor_readings[5, 10:], label="torque z [Nm]")
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.subplot(122)
plt.title("Deviations from desired pose")
plt.plot(pose_error[0, 10:], label="deviation x [m]")
plt.plot(pose_error[1, 10:], label="deviation y [m]")
plt.plot(pose_error[2, 10:], label="deviation z [m]")
plt.plot(pose_error[3, 10:], label="quaternion x")
plt.plot(pose_error[4, 10:], label="quaternion y")
plt.plot(pose_error[5, 10:], label="quaternion z")
plt.xlabel("number of iterations (adding up to 10 seconds)")
plt.legend()
plt.show()
if __name__ == "__main__":
rospy.init_node("ts_control_sim_only")
#rospy.on_shutdown(_on_shutdown)
publish_rate = 500
rate = rospy.Rate(publish_rate)
robot = ArmInterface()
print('Moving to starting position')
#robot.move_to_joint_positions(new_start)
robot.move_to_neutral()
impedance_control(rate, K_Pt, K_Po, K_m, K_d)
names = [
'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4',
'panda_joint5', 'panda_joint6', 'panda_joint7'
]
if __name__ == '__main__':
rospy.init_node("test_node")
r = ArmInterface()
rate = rospy.Rate(400)
elapsed_time_ = rospy.Duration(0.0)
period = rospy.Duration(0.005)
r.move_to_neutral() # move to neutral pose before beginning
initial_pose = deepcopy(r.joint_ordered_angles())
raw_input("Hit Enter to Start")
print "commanding"
vals = deepcopy(initial_pose)
count = 0
while not rospy.is_shutdown():
elapsed_time_ += period
delta = 3.14 / 16.0 * (
1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2
WARNING: The robot will move slightly (small arc swinging motion side-to-side) till code is killed.
"""
if __name__ == '__main__':
rospy.init_node("test_robot")
r = ArmInterface(
) # create arm interface instance (see https://justagist.github.io/franka_ros_interface/DOC.html#arminterface for all available methods for ArmInterface() object)
cm = r.get_controller_manager(
) # get controller manager instance associated with the robot (not required in most cases)
mvt = r.get_movegroup_interface(
) # get the moveit interface for planning and executing trajectories using moveit planners (see https://justagist.github.io/franka_ros_interface/DOC.html#franka_moveit.PandaMoveGroupInterface for documentation)
elapsed_time_ = rospy.Duration(0.0)
period = rospy.Duration(0.005)
r.move_to_neutral() # move robot to neutral pose
initial_pose = r.joint_angles() # get current joint angles of the robot
jac = r.zero_jacobian() # get end-effector jacobian
count = 0
rate = rospy.Rate(1000)
rospy.loginfo("Commanding...\n")
joint_names = r.joint_names()
vals = r.joint_angles()
while not rospy.is_shutdown():
elapsed_time_ += period
# /***************************************************************************
# Copyright (c) 2019-2020, Saif Sidhik
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# **************************************************************************/
"""
@info:
commands robot to move to neutral pose
"""
import rospy
from franka_interface import ArmInterface
if __name__ == '__main__':
rospy.init_node("move_to_neutral_node")
r = ArmInterface()
r.move_to_neutral()
