From f454b586a646003169d801f9deae09c6b712210a Mon Sep 17 00:00:00 2001
From: Pariterre <pariterre@hotmail.com>
Date: Fri, 10 Apr 2020 15:34:04 -0400
Subject: [PATCH] Allow for chaining while using setOnes, setZero or
 setIdentity

---
 include/rbdl/CasadiMath/MX_Xd_dynamic.h | 9 ++++++---
 include/rbdl/CasadiMath/MX_Xd_static.h  | 9 ++++++---
 2 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/include/rbdl/CasadiMath/MX_Xd_dynamic.h b/include/rbdl/CasadiMath/MX_Xd_dynamic.h
index 1639c9f..d7903ca 100644
--- a/include/rbdl/CasadiMath/MX_Xd_dynamic.h
+++ b/include/rbdl/CasadiMath/MX_Xd_dynamic.h
@@ -52,22 +52,25 @@ class MX_Xd_dynamic : public casadi::MX{
     static MX_Xd_dynamic Zero(unsigned int nrows, unsigned int ncols = 1){
         return casadi::MX::zeros(nrows, ncols);
     }
-    void setZero(){
+    MX_Xd_dynamic& setZero(){
         *this = casadi::MX::zeros(this->rows(), this->cols());
+        return *this;
     }
 
     static MX_Xd_dynamic One(unsigned int nrows, unsigned int ncols = 1){
         return casadi::MX::ones(nrows, ncols);
     }
-    void setOnes(){
+    MX_Xd_dynamic& setOnes(){
         *this = casadi::MX::ones(this->rows(), this->cols());
+        return *this;
     }
 
     static MX_Xd_dynamic Identity(unsigned int size, unsigned int ignoredSize = 0){
         return casadi::MX::eye(size);
     }
-    void setIdentity(){
+    MX_Xd_dynamic& setIdentity(){
         *this = casadi::MX::eye(cols());
+        return *this;
     }
 
     MX_Xd_SubMatrix operator[](unsigned int i) {
diff --git a/include/rbdl/CasadiMath/MX_Xd_static.h b/include/rbdl/CasadiMath/MX_Xd_static.h
index 6bfaaf5..3cbe223 100644
--- a/include/rbdl/CasadiMath/MX_Xd_static.h
+++ b/include/rbdl/CasadiMath/MX_Xd_static.h
@@ -253,22 +253,25 @@ class MX_Xd_static : public casadi::MX{
     static MX_Xd_static Identity(){
         return casadi::MX::eye(ncols);
     }
-    void setIdentity(){
+    MX_Xd_static& setIdentity(){
         *this = casadi::MX::eye(ncols);
+        return *this;
     }
 
     static MX_Xd_static Zero(){
         return MX_Xd_static<nrows, ncols>::zeros(nrows, ncols);
     }
-    void setZero(){
+    MX_Xd_static& setZero(){
         *this = casadi::MX::zeros(this->rows(), this->cols());
+        return *this;
     }
 
     static MX_Xd_static One(){
         return MX_Xd_static<nrows, ncols>::ones(nrows, ncols);
     }
-    void setOnes(){
+    MX_Xd_static& setOnes(){
         *this = casadi::MX::ones(this->rows(), this->cols());
+        return *this;
     }