Given a trajectory for arm , the generated speed fluctuates greatly, how to increase the smoothness of the speed generated by pink? could you please give an example to produce smooth velocity #101
Given a trajectory for arm , the generated joint speed fluctuates greatly, how to increase the smoothness of the joint speed generated by pink?
could you please give an example to produce smooth joint velocity
such as the demo arm_ur3.py ,produce joint velocity curve :
@stephane-caron @simeon-ned
We can see the joint velocity generated by pink is not smooth ,so how can we improve the velocity smooth generated by pink,
many thanks !
code is here
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# SPDX-License-Identifier: Apache-2.0
# Copyright 2022 Stéphane Caron
"""Universal Robots UR3 arm tracking a moving target."""
import os
import sys
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
import meshcat_shapes
import numpy as np
import qpsolvers
from loop_rate_limiters import RateLimiter
import os
import sys
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '../')))
import pink
from pink import solve_ik
from pink.tasks import FrameTask, PostureTask
from pink.utils import custom_configuration_vector
from pink.visualization import start_meshcat_visualizer
try:
from robot_descriptions.loaders.pinocchio import load_robot_description
except ModuleNotFoundError:
raise ModuleNotFoundError(
"Examples need robot_descriptions, "
"try ``pip install robot_descriptions``"
)
if __name__ == "__main__":
robot = load_robot_description("ur3_description", root_joint=None)
viz = start_meshcat_visualizer(robot)
end_effector_task = FrameTask(
"ee_link",
position_cost=1.0, # [cost] / [m]
orientation_cost=1.0, # [cost] / [rad]
lm_damping=1.0, # tuned for this setup
)
posture_task = PostureTask(
cost=1e-3, # [cost] / [rad]
)
tasks = [end_effector_task, posture_task]
q_ref = custom_configuration_vector(
robot,
shoulder_lift_joint=1.0,
shoulder_pan_joint=1.0,
elbow_joint=1.0,
)
configuration = pink.Configuration(robot.model, robot.data, q_ref)
for task in tasks:
task.set_target_from_configuration(configuration)
viz.display(configuration.q)
viewer = viz.viewer
meshcat_shapes.frame(viewer["end_effector_target"], opacity=0.5)
meshcat_shapes.frame(viewer["end_effector"], opacity=1.0)
# Select QP solver
solver = qpsolvers.available_solvers[0]
if "quadprog" in qpsolvers.available_solvers:
solver = "quadprog"
rate = RateLimiter(frequency=200.0)
dt = rate.period
t = 0.0 # [s]
#save data
vel_vec = []
q_vec = []
cnt = 0
cnt_max = 5000
while True and cnt < cnt_max:
# Update task targets
end_effector_target = end_effector_task.transform_target_to_world
end_effector_target.translation[1] = 0.5 + 0.1 * np.sin(2.0 * t)
end_effector_target.translation[2] = 0.2
# Update visualization frames
viewer["end_effector_target"].set_transform(end_effector_target.np)
viewer["end_effector"].set_transform(
configuration.get_transform_frame_to_world(
end_effector_task.frame
).np
)
# Compute velocity and integrate it into next configuration
velocity = solve_ik(configuration, tasks, dt, solver=solver)
configuration.integrate_inplace(velocity, dt)
vel_vec.append(velocity)
q_vec.append(configuration.q)
# Visualize result at fixed FPS
viz.display(configuration.q)
rate.sleep()
t += dt
cnt = cnt +1
#plot the data
import matplotlib.pyplot as plt
velocity_data = np.array(vel_vec)
configuration_data = np.array(q_vec)
plt.figure()
plt.plot(velocity_data)
plt.title("Velocity data")
plt.figure()
plt.plot(configuration_data)
plt.title("Configuration data")
plt.show()
Given a trajectory for arm , the generated joint speed fluctuates greatly, how to increase the smoothness of the joint speed generated by pink? could you please give an example to produce smooth joint velocity
such as the demo arm_ur3.py ,produce joint velocity curve :
@stephane-caron @simeon-ned We can see the joint velocity generated by pink is not smooth ,so how can we improve the velocity smooth generated by pink, many thanks !
code is here